diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 451ade3771f61..45fc1dd5176d6 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,9 +1,9 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai +common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_point_types/** taichi.higashide@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp @@ -44,23 +44,24 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_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 +control/autoware_joy_controller/** 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/autoware_lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp zulfaqar.azmi@tier4.jp +control/autoware_mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_smart_mpc_trajectory_follower/** kosuke.takeuchi@tier4.jp masayuki.aino@proxima-ai-tech.com takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@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 mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@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 -control/joy_controller/** 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/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp -control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp 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/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/predicted_path_checker/** berkay@leodrive.ai -control/pure_pursuit/** takamasa.horibe@tier4.jp -control/shift_decider/** takamasa.horibe@tier4.jp -control/smart_mpc_trajectory_follower/** masayuki.aino@proxima-ai-tech.com -control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_smart_mpc_trajectory_follower/** masayuki.aino@proxima-ai-tech.com +control/autoware_trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/pure_pursuit/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/shift_decider/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp evaluator/control_evaluator/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp evaluator/kinematic_evaluator/** dominik.jargot@robotec.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 @@ -70,7 +71,7 @@ evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.j launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -launch/tier4_map_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +launch/tier4_map_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp @@ -99,14 +100,15 @@ localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tie localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/util/lanelet2_map_preprocessor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +perception/autoware_crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/autoware_map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp yoshi.ri@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 shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp perception/detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -119,7 +121,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 koji.minoda@tier4.jp perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp -perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp +perception/lidar_transfusion/** amadeusz.szymko.2@tier4.jp kenzo.lobos@tier4.jp satoshi.tanaka@tier4.jp perception/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -144,68 +146,81 @@ perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong 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_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp +planning/autoware_behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_path_static_obstacle_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_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/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp +planning/autoware_behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/autoware_external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/autoware_freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/autoware_mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/autoware_objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_path_optimizer/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/autoware_path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp +planning/autoware_planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp +planning/autoware_planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai +planning/autoware_rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp +planning/autoware_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/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp -planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp -planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_surround_obstacle_checker/** go.sakayori@tier4.jp satoshi.ota@tier4.jp +planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp -planning/behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp +planning/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@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/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp +planning/autoware_behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@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 -planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@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/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp -planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@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 -planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp -planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp -planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp +planning/autoware_objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai bnk@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_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp -planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp -planning/route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@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 -planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp -planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp -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/surround_obstacle_checker/** satoshi.ota@tier4.jp +planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.jp +planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp +planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp +planning/sampling_based_planner/autoware_sampler_common/** maxime.clement@tier4.jp sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp sensing/image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp -sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp @@ -239,10 +254,10 @@ system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp tools/reaction_analyzer/** berkay@leodrive.ai -vehicle/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp -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 +vehicle/autoware_accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp +vehicle/autoware_external_cmd_converter/** eiki.nagata.2@tier4.jp takamasa.horibe@tier4.jp +vehicle/autoware_raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp +vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp +vehicle/autoware_vehicle_info_utils/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp ### Copied from .github/CODEOWNERS-manual ### diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index fb98d321fde88..6745976aa543a 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -74,40 +74,3 @@ jobs: - name: Show disk space after the tasks run: df -h - - clang-tidy-differential: - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda - steps: - - name: Check out repository - uses: actions/checkout@v4 - with: - fetch-depth: 0 - - - name: Show disk space before the tasks - run: df -h - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - - name: Get modified files - id: get-modified-files - uses: tj-actions/changed-files@v35 - with: - files: | - **/*.cpp - **/*.hpp - - - name: Run clang-tidy - if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 - with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy - build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index a469e7a33d190..85c531a02fca9 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -9,7 +9,7 @@ on: jobs: build-and-test: if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }} - runs-on: ubuntu-latest + runs-on: [self-hosted, linux, X64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml new file mode 100644 index 0000000000000..26c5314d4b119 --- /dev/null +++ b/.github/workflows/clang-tidy-differential.yaml @@ -0,0 +1,53 @@ +name: clang-tidy-differential + +on: + pull_request: + types: + - opened + - synchronize + - labeled + +jobs: + prevent-no-label-execution: + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + with: + label: tag:run-clang-tidy-differential + + clang-tidy-differential: + needs: prevent-no-label-execution + if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} + runs-on: ubuntu-latest + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Show disk space before the tasks + run: df -h + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 + + - name: Get modified packages + id: get-modified-packages + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Get modified files + id: get-modified-files + uses: tj-actions/changed-files@v35 + with: + files: | + **/*.cpp + **/*.hpp + + - name: Run clang-tidy + if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + with: + rosdistro: humble + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy + build-depends-repos: build_depends.repos diff --git a/.github/workflows/cppcheck-all.yaml b/.github/workflows/cppcheck-all.yaml deleted file mode 100644 index db3bd5d259895..0000000000000 --- a/.github/workflows/cppcheck-all.yaml +++ /dev/null @@ -1,60 +0,0 @@ -name: cppcheck-all - -on: - pull_request: - schedule: - - cron: 0 0 * * * - workflow_dispatch: - -jobs: - cppcheck-all: - runs-on: ubuntu-latest - - steps: - - name: Checkout code - uses: actions/checkout@v2 - - - name: Install dependencies - run: | - sudo apt-get update - sudo apt-get install -y build-essential cmake git libpcre3-dev - - # cppcheck from apt does not yet support --check-level args, and thus install from source - - name: Install Cppcheck from source - run: | - mkdir /tmp/cppcheck - git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck - cd /tmp/cppcheck - git checkout 2.14.1 - mkdir build - cd build - cmake .. - make -j $(nproc) - sudo make install - - - name: Run Cppcheck on all files - continue-on-error: true - id: cppcheck - run: | - cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --xml . 2> cppcheck-report.xml - shell: bash - - - name: Count errors by error ID and severity - run: | - #!/bin/bash - temp_file=$(mktemp) - grep -oP '(?<=id=")[^"]+" severity="[^"]+' cppcheck-report.xml | sed 's/" severity="/,/g' > "$temp_file" - echo "Error counts by error ID and severity:" - sort "$temp_file" | uniq -c - rm "$temp_file" - shell: bash - - - name: Upload Cppcheck report - uses: actions/upload-artifact@v2 - with: - name: cppcheck-report - path: cppcheck-report.xml - - - name: Fail the job if Cppcheck failed - if: steps.cppcheck.outcome == 'failure' - run: exit 1 diff --git a/.github/workflows/cppcheck-daily.yaml b/.github/workflows/cppcheck-daily.yaml new file mode 100644 index 0000000000000..6a18bfb988b08 --- /dev/null +++ b/.github/workflows/cppcheck-daily.yaml @@ -0,0 +1,59 @@ +name: cppcheck-daily + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + cppcheck-daily: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Run Cppcheck on all files + continue-on-error: true + id: cppcheck + run: | + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --xml . 2> cppcheck-report.xml + shell: bash + + - name: Count errors by error ID and severity + run: | + #!/bin/bash + temp_file=$(mktemp) + grep -oP '(?<=id=")[^"]+" severity="[^"]+' cppcheck-report.xml | sed 's/" severity="/,/g' > "$temp_file" + echo "Error counts by error ID and severity:" + sort "$temp_file" | uniq -c + rm "$temp_file" + shell: bash + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.xml + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/openai-pr-reviewer.yaml b/.github/workflows/openai-pr-reviewer.yaml deleted file mode 100644 index 4ecbd93198276..0000000000000 --- a/.github/workflows/openai-pr-reviewer.yaml +++ /dev/null @@ -1,39 +0,0 @@ -name: Code Review By ChatGPT - -permissions: - contents: read - pull-requests: write - -on: - pull_request_target: - types: - - labeled - pull_request_review_comment: - types: [created] - -concurrency: - group: ${{ github.repository }}-${{ github.event.number || github.head_ref || - github.sha }}-${{ github.workflow }}-${{ github.event_name == - 'pull_request_review_comment' && 'pr_comment' || 'pr' }} - cancel-in-progress: ${{ github.event_name != 'pull_request_review_comment' }} - -jobs: - prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 - with: - label: tag:openai-pr-reviewer - review: - needs: prevent-no-label-execution - if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} - runs-on: ubuntu-latest - steps: - - uses: fluxninja/openai-pr-reviewer@latest - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - OPENAI_API_KEY: ${{ secrets.OPENAI_API_KEY }} - with: - debug: false - review_simple_changes: false - review_comment_lgtm: false - openai_light_model: gpt-3.5-turbo-0613 - openai_heavy_model: gpt-3.5-turbo-0613 # The default is to use GPT4, which needs to be changed to join ChatGPT Plus. diff --git a/.github/workflows/pr-agent.yaml b/.github/workflows/pr-agent.yaml index f4794b7497402..81b192a1c2095 100644 --- a/.github/workflows/pr-agent.yaml +++ b/.github/workflows/pr-agent.yaml @@ -26,6 +26,13 @@ jobs: env: OPENAI_KEY: ${{ secrets.OPENAI_KEY_PR_AGENT }} GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - github_action_config.auto_review: "false" - github_action_config.auto_describe: "false" - github_action_config.auto_improve: "false" + github_action_config.auto_review: false + github_action_config.auto_describe: false + github_action_config.auto_improve: false + # https://github.com/Codium-ai/pr-agent/blob/main/pr_agent/settings/configuration.toml + pr_description.publish_labels: false + config.model: gpt-4o + config.model_turbo: gpt-4o + config.max_model_tokens: 64000 + pr_code_suggestions.max_context_tokens: 12000 + pr_code_suggestions.commitable_code_suggestions: true diff --git a/build_depends.repos b/build_depends.repos index a8f431f97df25..bd93897d81d68 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -41,3 +41,7 @@ repositories: type: git url: https://github.com/tier4/glog.git version: v0.6.0_t4-ros + universe/external/ament_cmake: # TODO(mitsudome-r): remove when https://github.com/ament/ament_cmake/pull/448 is merged + type: git + url: https://github.com/autowarefoundation/ament_cmake.git + version: feat/faster_ament_libraries_deduplicate diff --git a/common/global_parameter_loader/launch/global_params.launch.py b/common/global_parameter_loader/launch/global_params.launch.py index 06f807aaa4609..8f9092b10fa37 100644 --- a/common/global_parameter_loader/launch/global_params.launch.py +++ b/common/global_parameter_loader/launch/global_params.launch.py @@ -33,7 +33,7 @@ def launch_setup(context, *args, **kwargs): load_vehicle_info = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("vehicle_info_util"), "/launch/vehicle_info.launch.py"] + [FindPackageShare("autoware_vehicle_info_utils"), "/launch/vehicle_info.launch.py"] ), launch_arguments={ "vehicle_info_param_file": [vehicle_description_pkg, "/config/vehicle_info.param.yaml"] diff --git a/common/global_parameter_loader/package.xml b/common/global_parameter_loader/package.xml index 4c2568b9aec98..472ef0c063430 100644 --- a/common/global_parameter_loader/package.xml +++ b/common/global_parameter_loader/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - vehicle_info_util + autoware_vehicle_info_utils ament_lint_auto autoware_lint_common diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp index b842261d56cfa..d0b6dfac2dcff 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp @@ -20,13 +20,27 @@ #include #include #include +#include +#include namespace tier4_autoware_utils { -template -class InterProcessPollingSubscriber +template +class InterProcessPollingSubscriber; + +template +class InterProcessPollingSubscriber::type> { +public: + using SharedPtr = + std::shared_ptr::type>>; + static SharedPtr create_subscription( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) + { + return std::make_shared>(node, topic_name, qos); + } + private: typename rclcpp::Subscription::SharedPtr subscriber_; typename T::SharedPtr data_; @@ -50,6 +64,12 @@ class InterProcessPollingSubscriber "serialization while updateLatestData()"); } }; + /* + * @brief take and return the latest data from DDS queue if such data existed, otherwise return + * previous taken data("stale" data) + * @note if you want to ignore "stale" data, you should use takeNewData() + * instead + */ typename T::ConstSharedPtr takeData() { auto new_data = std::make_shared(); @@ -61,6 +81,75 @@ class InterProcessPollingSubscriber return data_; }; + + /* + * @brief take and return the latest data from DDS queue if such data existed, otherwise return + * nullptr instead + * @note this API allows you to avoid redundant computation on the taken data which is unchanged + * since the previous cycle + */ + typename T::ConstSharedPtr takeNewData() + { + auto new_data = std::make_shared(); + rclcpp::MessageInfo message_info; + const bool success = subscriber_->take(*new_data, message_info); + if (success) { + data_ = new_data; + return data_; + } else { + return nullptr; + } + } +}; + +template +class InterProcessPollingSubscriber= 2)>::type> +{ +public: + using SharedPtr = + std::shared_ptr= 2)>::type>>; + static SharedPtr create_subscription( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{N}) + { + return std::make_shared>(node, topic_name, qos); + } + +private: + typename rclcpp::Subscription::SharedPtr subscriber_; + +public: + explicit InterProcessPollingSubscriber( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{N}) + { + auto noexec_callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto noexec_subscription_options = rclcpp::SubscriptionOptions(); + noexec_subscription_options.callback_group = noexec_callback_group; + + subscriber_ = node->create_subscription( + topic_name, qos, + [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); }, + noexec_subscription_options); + if (qos.get_rmw_qos_profile().depth < N) { + throw std::invalid_argument( + "InterProcessPollingSubscriber will be used with depth == " + std::to_string(N) + + ", which may cause inefficient serialization while updateLatestData()"); + } + }; + std::vector takeData() + { + std::vector data; + rclcpp::MessageInfo message_info; + for (int i = 0; i < N; ++i) { + auto datum = std::make_shared(); + if (subscriber_->take(*datum, message_info)) { + data.push_back(datum); + } else { + break; + } + } + return data; + }; }; } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp index ffc845575a76f..96b1cc6ea6ccc 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp @@ -48,8 +48,9 @@ class TransformListener try { tf = tf_buffer_->lookupTransform(from, to, tf2::TimePointZero); } catch (tf2::TransformException & ex) { - RCLCPP_WARN( - logger_, "failed to get transform from %s to %s: %s", from.c_str(), to.c_str(), ex.what()); + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, "failed to get transform from %s to %s: %s", from.c_str(), + to.c_str(), ex.what()); return {}; } @@ -64,8 +65,9 @@ class TransformListener try { tf = tf_buffer_->lookupTransform(from, to, time, duration); } catch (tf2::TransformException & ex) { - RCLCPP_WARN( - logger_, "failed to get transform from %s to %s: %s", from.c_str(), to.c_str(), ex.what()); + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, "failed to get transform from %s to %s: %s", from.c_str(), + to.c_str(), ex.what()); return {}; } diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index f2482b2d45a3b..1c1c4ad6d8ba1 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto autoware_cmake + autoware_vehicle_info_utils geometry_msgs libqt5-core libqt5-gui @@ -20,7 +21,6 @@ rviz_common rviz_default_plugins tf2_ros - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp index c4233bd6305e3..cd32df498bfd4 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp @@ -172,7 +172,7 @@ void PoseHistoryFootprint::updateFootprint() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_THROTTLE( diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp index d957054bd479e..44625ad7deb41 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp @@ -15,8 +15,8 @@ #ifndef POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ #define POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ +#include #include -#include #include @@ -39,8 +39,8 @@ class BoolProperty; namespace rviz_plugins { -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfoUtils; class PoseHistoryFootprint : public rviz_common::MessageFilterDisplay diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index ed79e122c0a01..9cb710c239029 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -15,6 +15,7 @@ #ifndef TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ #define TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ +#include #include #include #include @@ -25,7 +26,6 @@ #include #include #include -#include #include @@ -97,8 +97,8 @@ bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) namespace rviz_plugins { -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfoUtils; template class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay { diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 5d755d212c859..95edcb7a96c56 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_planning_msgs + autoware_vehicle_info_utils libqt5-core libqt5-gui libqt5-widgets @@ -25,7 +26,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 4a297b47bcfcc..342e72d66c5f9 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -31,7 +31,7 @@ void AutowarePathWithLaneIdDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( @@ -107,7 +107,7 @@ void AutowarePathDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( @@ -124,7 +124,7 @@ void AutowareTrajectoryDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( diff --git a/control/autonomous_emergency_braking/CMakeLists.txt b/control/autonomous_emergency_braking/CMakeLists.txt deleted file mode 100644 index 53a629fafa0cc..0000000000000 --- a/control/autonomous_emergency_braking/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autonomous_emergency_braking) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(PCL REQUIRED) - -include_directories( - include - SYSTEM - ${PCL_INCLUDE_DIRS} -) - -set(AEB_NODE ${PROJECT_NAME}_node) -ament_auto_add_library(${AEB_NODE} SHARED - src/node.cpp -) - -rclcpp_components_register_node(${AEB_NODE} - PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB" - EXECUTABLE ${PROJECT_NAME} -) - -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/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp deleted file mode 100644 index 81ab2ecff790b..0000000000000 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ /dev/null @@ -1,343 +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 AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ -#define AUTONOMOUS_EMERGENCY_BRAKING__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 -#include -#include -#include -#include -#include -namespace autoware::motion::control::autonomous_emergency_braking -{ - -using autoware_planning_msgs::msg::Trajectory; -using autoware_system_msgs::msg::AutowareState; -using autoware_vehicle_msgs::msg::VelocityReport; -using nav_msgs::msg::Odometry; -using sensor_msgs::msg::Imu; -using sensor_msgs::msg::PointCloud2; -using PointCloud = pcl::PointCloud; -using diagnostic_updater::DiagnosticStatusWrapper; -using diagnostic_updater::Updater; -using tier4_autoware_utils::Point2d; -using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; -using visualization_msgs::msg::Marker; -using visualization_msgs::msg::MarkerArray; -using Path = std::vector; -using Vector3 = geometry_msgs::msg::Vector3; -struct ObjectData -{ - rclcpp::Time stamp; - geometry_msgs::msg::Point position; - double velocity{0.0}; - double rss{0.0}; - double distance_to_object{0.0}; -}; - -class CollisionDataKeeper -{ -public: - explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } - - void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time) - { - collision_keep_time_ = collision_keep_time; - previous_obstacle_keep_time_ = previous_obstacle_keep_time; - } - - std::pair getTimeout() - { - return {collision_keep_time_, previous_obstacle_keep_time_}; - } - - bool checkObjectDataExpired(std::optional & data, const double timeout) - { - if (!data.has_value()) return true; - const auto now = clock_->now(); - const auto & prev_obj = data.value(); - const auto & data_time_stamp = prev_obj.stamp; - if ((now - data_time_stamp).nanoseconds() * 1e-9 > timeout) { - data = std::nullopt; - return true; - } - return false; - } - - bool checkCollisionExpired() - { - return this->checkObjectDataExpired(closest_object_, collision_keep_time_); - } - - bool checkPreviousObjectDataExpired() - { - return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); - } - - ObjectData get() const - { - return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); - } - - ObjectData getPreviousObjectData() const - { - return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); - } - - void setCollisionData(const ObjectData & data) - { - closest_object_ = std::make_optional(data); - } - - void setPreviousObjectData(const ObjectData & data) - { - prev_closest_object_ = std::make_optional(data); - } - - void resetVelocityHistory() { obstacle_velocity_history_.clear(); } - - void updateVelocityHistory( - const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp) - { - // remove old msg from deque - const auto now = clock_->now(); - obstacle_velocity_history_.erase( - std::remove_if( - obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(), - [&](const auto & velocity_time_pair) { - const auto & vel_time = velocity_time_pair.second; - return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_); - }), - obstacle_velocity_history_.end()); - obstacle_velocity_history_.emplace_back( - std::make_pair(current_object_velocity, current_object_velocity_time_stamp)); - } - - std::optional getMedianObstacleVelocity() const - { - if (obstacle_velocity_history_.empty()) return std::nullopt; - std::vector raw_velocities; - for (const auto & vel_time_pair : obstacle_velocity_history_) { - raw_velocities.emplace_back(vel_time_pair.first); - } - - const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1 - : (raw_velocities.size()) / 2.0; - const size_t med2 = (raw_velocities.size()) / 2.0; - std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end()); - const double vel1 = raw_velocities.at(med1); - std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end()); - const double vel2 = raw_velocities.at(med2); - return (vel1 + vel2) / 2.0; - } - - std::optional calcObjectSpeedFromHistory( - const ObjectData & closest_object, const Path & path, const double current_ego_speed) - { - if (this->checkPreviousObjectDataExpired()) { - this->setPreviousObjectData(closest_object); - this->resetVelocityHistory(); - return std::nullopt; - } - - const auto estimated_velocity_opt = std::invoke([&]() -> std::optional { - const auto & prev_object = this->getPreviousObjectData(); - const double p_dt = - (closest_object.stamp.nanoseconds() - prev_object.stamp.nanoseconds()) * 1e-9; - if (p_dt < std::numeric_limits::epsilon()) return std::nullopt; - const auto & nearest_collision_point = closest_object.position; - const auto & prev_collision_point = prev_object.position; - - const double p_dx = nearest_collision_point.x - prev_collision_point.x; - const double p_dy = nearest_collision_point.y - prev_collision_point.y; - const double p_dist = std::hypot(p_dx, p_dy); - const double p_yaw = std::atan2(p_dy, p_dx); - const double p_vel = p_dist / p_dt; - - const auto nearest_idx = motion_utils::findNearestIndex(path, nearest_collision_point); - const auto & nearest_path_pose = path.at(nearest_idx); - const auto & traj_yaw = tf2::getYaw(nearest_path_pose.orientation); - const auto estimated_velocity = p_vel * std::cos(p_yaw - traj_yaw) + current_ego_speed; - - // Current RSS distance calculation does not account for negative velocities - return (estimated_velocity > 0.0) ? estimated_velocity : 0.0; - }); - - if (!estimated_velocity_opt.has_value()) { - return std::nullopt; - } - - const auto & estimated_velocity = estimated_velocity_opt.value(); - this->setPreviousObjectData(closest_object); - this->updateVelocityHistory(estimated_velocity, closest_object.stamp); - return this->getMedianObstacleVelocity(); - } - -private: - std::optional prev_closest_object_{std::nullopt}; - std::optional closest_object_{std::nullopt}; - double collision_keep_time_{0.0}; - double previous_obstacle_keep_time_{0.0}; - - std::deque> obstacle_velocity_history_; - rclcpp::Clock::SharedPtr clock_; -}; - -static rclcpp::SensorDataQoS SingleDepthSensorQoS() -{ - rclcpp::SensorDataQoS qos; - qos.get_rmw_qos_profile().depth = 1; - return qos; -} - -class AEB : public rclcpp::Node -{ -public: - explicit AEB(const rclcpp::NodeOptions & node_options); - - // subscriber - tier4_autoware_utils::InterProcessPollingSubscriber sub_point_cloud_{ - this, "~/input/pointcloud", SingleDepthSensorQoS()}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_velocity_{ - this, "~/input/velocity"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_traj_{ - this, "~/input/predicted_trajectory"}; - tier4_autoware_utils::InterProcessPollingSubscriber sub_autoware_state_{ - this, "/autoware/state"}; - // publisher - rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; - rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug - - // timer - rclcpp::TimerBase::SharedPtr timer_; - - // callback - void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); - void onImu(const Imu::ConstSharedPtr input_msg); - void onTimer(); - rcl_interfaces::msg::SetParametersResult onParameter( - const std::vector & parameters); - - bool fetchLatestData(); - - // main function - void onCheckCollision(DiagnosticStatusWrapper & stat); - bool checkCollision(MarkerArray & debug_markers); - bool hasCollision(const double current_v, const ObjectData & closest_object); - - Path generateEgoPath(const double curr_v, const double curr_w); - std::optional generateEgoPath(const Trajectory & predicted_traj); - std::vector generatePathFootprint(const Path & path, const double extra_width_margin); - - void createObjectDataUsingPointCloudClusters( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects, - const pcl::PointCloud::Ptr obstacle_points_ptr); - - void cropPointCloudWithEgoFootprintPath( - const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); - - void createObjectDataUsingPointCloudClusters( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects); - void cropPointCloudWithEgoFootprintPath(const std::vector & ego_polys); - - void addMarker( - const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, - const std::vector & objects, const std::optional & closest_object, - const double color_r, const double color_g, const double color_b, const double color_a, - const std::string & ns, MarkerArray & debug_markers); - - void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); - - std::optional calcObjectSpeedFromHistory( - const ObjectData & closest_object, const Path & path, const double current_ego_speed); - - PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; - VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; - Vector3::SharedPtr angular_velocity_ptr_{nullptr}; - Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; - AutowareState::ConstSharedPtr autoware_state_{nullptr}; - - tf2_ros::Buffer tf_buffer_{get_clock()}; - tf2_ros::TransformListener tf_listener_{tf_buffer_}; - - // vehicle info - VehicleInfo vehicle_info_; - - // diag - Updater updater_{this}; - - // member variables - bool publish_debug_pointcloud_; - bool use_predicted_trajectory_; - bool use_imu_path_; - double path_footprint_extra_margin_; - double detection_range_min_height_; - double detection_range_max_height_margin_; - double voxel_grid_x_; - double voxel_grid_y_; - double voxel_grid_z_; - double min_generated_path_length_; - double expand_width_; - double longitudinal_offset_; - double t_response_; - double a_ego_min_; - double a_obj_min_; - double cluster_tolerance_; - int minimum_cluster_size_; - int maximum_cluster_size_; - double imu_prediction_time_horizon_; - double imu_prediction_time_interval_; - double mpc_prediction_time_horizon_; - double mpc_prediction_time_interval_; - CollisionDataKeeper collision_data_keeper_; - // Parameter callback - OnSetParametersCallbackHandle::SharedPtr set_param_res_; -}; -} // namespace autoware::motion::control::autonomous_emergency_braking - -#endif // AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ diff --git a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml b/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml deleted file mode 100644 index cf6640ec6be52..0000000000000 --- a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml deleted file mode 100644 index bf27e6b7e1575..0000000000000 --- a/control/autonomous_emergency_braking/package.xml +++ /dev/null @@ -1,46 +0,0 @@ - - - - autonomous_emergency_braking - 0.1.0 - Autonomous Emergency Braking package as a ROS 2 node - Takamasa Horibe - Tomoya Kimura - Mamoru Sobue - Daniel Sanchez - - Apache License 2.0 - - ament_cmake - autoware_cmake - - autoware_control_msgs - autoware_planning_msgs - autoware_system_msgs - autoware_vehicle_msgs - diagnostic_updater - geometry_msgs - motion_utils - nav_msgs - pcl_conversions - pcl_ros - pointcloud_preprocessor - rclcpp - rclcpp_components - sensor_msgs - std_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - vehicle_info_util - visualization_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp deleted file mode 100644 index 71eb92a4e95fb..0000000000000 --- a/control/autonomous_emergency_braking/src/node.cpp +++ /dev/null @@ -1,742 +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 "autonomous_emergency_braking/node.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - -namespace autoware::motion::control::autonomous_emergency_braking -{ -using diagnostic_msgs::msg::DiagnosticStatus; -namespace bg = boost::geometry; - -void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) -{ - Point2d point; - point.x() = geom_point.x; - point.y() = geom_point.y; - - bg::append(polygon.outer(), point); -} - -Polygon2d createPolygon( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_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; - - appendPointToPolygon( - polygon, - tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); - appendPointToPolygon( - polygon, - tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); - - appendPointToPolygon( - polygon, - tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); - appendPointToPolygon( - polygon, - tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); - appendPointToPolygon( - polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); - - polygon = tier4_autoware_utils::isClockwise(polygon) - ? polygon - : tier4_autoware_utils::inverseClockwise(polygon); - - Polygon2d hull_polygon; - bg::convex_hull(polygon, hull_polygon); - - return hull_polygon; -} - -AEB::AEB(const rclcpp::NodeOptions & node_options) -: Node("AEB", node_options), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), - collision_data_keeper_(this->get_clock()) -{ - // Publisher - { - pub_obstacle_pointcloud_ = - this->create_publisher("~/debug/obstacle_pointcloud", 1); - debug_ego_path_publisher_ = this->create_publisher("~/debug/markers", 1); - } - // Diagnostics - { - updater_.setHardwareID("autonomous_emergency_braking"); - updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision); - } - // parameter - publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); - use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); - use_imu_path_ = declare_parameter("use_imu_path"); - path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); - detection_range_min_height_ = declare_parameter("detection_range_min_height"); - detection_range_max_height_margin_ = - declare_parameter("detection_range_max_height_margin"); - voxel_grid_x_ = declare_parameter("voxel_grid_x"); - voxel_grid_y_ = declare_parameter("voxel_grid_y"); - voxel_grid_z_ = declare_parameter("voxel_grid_z"); - min_generated_path_length_ = declare_parameter("min_generated_path_length"); - expand_width_ = declare_parameter("expand_width"); - longitudinal_offset_ = declare_parameter("longitudinal_offset"); - t_response_ = declare_parameter("t_response"); - a_ego_min_ = declare_parameter("a_ego_min"); - a_obj_min_ = declare_parameter("a_obj_min"); - - cluster_tolerance_ = declare_parameter("cluster_tolerance"); - minimum_cluster_size_ = declare_parameter("minimum_cluster_size"); - maximum_cluster_size_ = declare_parameter("maximum_cluster_size"); - - imu_prediction_time_horizon_ = declare_parameter("imu_prediction_time_horizon"); - imu_prediction_time_interval_ = declare_parameter("imu_prediction_time_interval"); - mpc_prediction_time_horizon_ = declare_parameter("mpc_prediction_time_horizon"); - mpc_prediction_time_interval_ = declare_parameter("mpc_prediction_time_interval"); - - { // Object history data keeper setup - const auto previous_obstacle_keep_time = - declare_parameter("previous_obstacle_keep_time"); - const auto collision_keeping_sec = declare_parameter("collision_keeping_sec"); - collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); - } - - // Parameter Callback - set_param_res_ = - add_on_set_parameters_callback(std::bind(&AEB::onParameter, this, std::placeholders::_1)); - - // start time - const double aeb_hz = declare_parameter("aeb_hz"); - const auto period_ns = rclcpp::Rate(aeb_hz).period(); - timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this)); -} - -rcl_interfaces::msg::SetParametersResult AEB::onParameter( - const std::vector & parameters) -{ - using tier4_autoware_utils::updateParam; - updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); - updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); - updateParam(parameters, "use_imu_path", use_imu_path_); - updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); - updateParam(parameters, "detection_range_min_height", detection_range_min_height_); - updateParam( - parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); - updateParam(parameters, "voxel_grid_x", voxel_grid_x_); - updateParam(parameters, "voxel_grid_y", voxel_grid_y_); - updateParam(parameters, "voxel_grid_z", voxel_grid_z_); - updateParam(parameters, "min_generated_path_length", min_generated_path_length_); - updateParam(parameters, "expand_width", expand_width_); - updateParam(parameters, "longitudinal_offset", longitudinal_offset_); - updateParam(parameters, "t_response", t_response_); - updateParam(parameters, "a_ego_min", a_ego_min_); - updateParam(parameters, "a_obj_min", a_obj_min_); - - updateParam(parameters, "cluster_tolerance", cluster_tolerance_); - updateParam(parameters, "minimum_cluster_size", minimum_cluster_size_); - updateParam(parameters, "maximum_cluster_size", maximum_cluster_size_); - - updateParam(parameters, "imu_prediction_time_horizon", imu_prediction_time_horizon_); - updateParam(parameters, "imu_prediction_time_interval", imu_prediction_time_interval_); - updateParam(parameters, "mpc_prediction_time_horizon", mpc_prediction_time_horizon_); - updateParam(parameters, "mpc_prediction_time_interval", mpc_prediction_time_interval_); - - { // Object history data keeper setup - auto [previous_obstacle_keep_time, collision_keeping_sec] = collision_data_keeper_.getTimeout(); - updateParam(parameters, "previous_obstacle_keep_time", previous_obstacle_keep_time); - updateParam(parameters, "collision_keeping_sec", collision_keeping_sec); - collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); - } - - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - return result; -} - -void AEB::onTimer() -{ - updater_.force_update(); -} - -void AEB::onImu(const Imu::ConstSharedPtr input_msg) -{ - // transform imu - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", input_msg->header.frame_id, input_msg->header.stamp, - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM( - get_logger(), - "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); - return; - } - - angular_velocity_ptr_ = std::make_shared(); - tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); -} - -void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) -{ - PointCloud::Ptr pointcloud_ptr(new PointCloud); - pcl::fromROSMsg(*input_msg, *pointcloud_ptr); - - if (input_msg->header.frame_id != "base_link") { - RCLCPP_ERROR_STREAM( - get_logger(), - "[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id); - // transform pointcloud - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", input_msg->header.frame_id, input_msg->header.stamp, - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM( - get_logger(), - "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); - return; - } - - // transform by using eigen matrix - PointCloud2 transformed_points{}; - const Eigen::Matrix4f affine_matrix = - tf2::transformToEigen(transform_stamped.transform).matrix().cast(); - pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points); - pcl::fromROSMsg(transformed_points, *pointcloud_ptr); - } - - // apply z-axis filter for removing False Positive points - PointCloud::Ptr height_filtered_pointcloud_ptr(new PointCloud); - pcl::PassThrough height_filter; - height_filter.setInputCloud(pointcloud_ptr); - height_filter.setFilterFieldName("z"); - height_filter.setFilterLimits( - detection_range_min_height_, - vehicle_info_.vehicle_height_m + detection_range_max_height_margin_); - height_filter.filter(*height_filtered_pointcloud_ptr); - - pcl::VoxelGrid filter; - PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud); - filter.setInputCloud(height_filtered_pointcloud_ptr); - filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_); - filter.filter(*no_height_filtered_pointcloud_ptr); - - obstacle_ros_pointcloud_ptr_ = std::make_shared(); - - pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); - obstacle_ros_pointcloud_ptr_->header = input_msg->header; -} - -bool AEB::fetchLatestData() -{ - const auto missing = [this](const auto & name) { - RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name); - return false; - }; - - current_velocity_ptr_ = sub_velocity_.takeData(); - if (!current_velocity_ptr_) { - return missing("ego velocity"); - } - - const auto pointcloud_ptr = sub_point_cloud_.takeData(); - if (!pointcloud_ptr) { - return missing("object pointcloud message"); - } - onPointCloud(pointcloud_ptr); - if (!obstacle_ros_pointcloud_ptr_) { - return missing("object pointcloud"); - } - - const auto imu_ptr = sub_imu_.takeData(); - if (use_imu_path_) { - if (!imu_ptr) { - return missing("imu message"); - } - // imu_ptr is valid - onImu(imu_ptr); - } - if (use_imu_path_ && !angular_velocity_ptr_) { - return missing("imu"); - } - - predicted_traj_ptr_ = sub_predicted_traj_.takeData(); - if (use_predicted_trajectory_ && !predicted_traj_ptr_) { - return missing("control predicted trajectory"); - } - - autoware_state_ = sub_autoware_state_.takeData(); - if (!autoware_state_) { - return missing("autoware_state"); - } - - return true; -} - -void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) -{ - MarkerArray debug_markers; - checkCollision(debug_markers); - - if (!collision_data_keeper_.checkCollisionExpired()) { - const std::string error_msg = "[AEB]: Emergency Brake"; - const auto diag_level = DiagnosticStatus::ERROR; - stat.summary(diag_level, error_msg); - const auto & data = collision_data_keeper_.get(); - stat.addf("RSS", "%.2f", data.rss); - stat.addf("Distance", "%.2f", data.distance_to_object); - stat.addf("Object Speed", "%.2f", data.velocity); - addCollisionMarker(data, debug_markers); - } else { - const std::string error_msg = "[AEB]: No Collision"; - const auto diag_level = DiagnosticStatus::OK; - stat.summary(diag_level, error_msg); - } - - // publish debug markers - debug_ego_path_publisher_->publish(debug_markers); -} - -bool AEB::checkCollision(MarkerArray & debug_markers) -{ - using colorTuple = std::tuple; - - // step1. check data - if (!fetchLatestData()) { - return false; - } - - // if not driving, disable aeb - if (autoware_state_->state != AutowareState::DRIVING) { - return false; - } - - // step2. create velocity data check if the vehicle stops or not - const double current_v = current_velocity_ptr_->longitudinal_velocity; - if (current_v < 0.1) { - return false; - } - - auto check_collision = [&]( - const auto & path, const colorTuple & debug_colors, - const std::string & debug_ns, - pcl::PointCloud::Ptr filtered_objects) { - // Crop out Pointcloud using an extra wide ego path - const auto expanded_ego_polys = - generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_); - cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects); - - // Check which points of the cropped point cloud are on the ego path, and get the closest one - std::vector objects_from_point_clusters; - const auto ego_polys = generatePathFootprint(path, expand_width_); - const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; - createObjectDataUsingPointCloudClusters( - path, ego_polys, current_time, objects_from_point_clusters, filtered_objects); - - // Get only the closest object and calculate its speed - const auto closest_object_point = std::invoke([&]() -> std::optional { - const auto closest_object_point_itr = std::min_element( - objects_from_point_clusters.begin(), objects_from_point_clusters.end(), - [](const auto & o1, const auto & o2) { - return o1.distance_to_object < o2.distance_to_object; - }); - - if (closest_object_point_itr == objects_from_point_clusters.end()) { - return std::nullopt; - } - const auto closest_object_speed = collision_data_keeper_.calcObjectSpeedFromHistory( - *closest_object_point_itr, path, current_v); - if (!closest_object_speed.has_value()) { - return std::nullopt; - } - closest_object_point_itr->velocity = closest_object_speed.value(); - return std::make_optional(*closest_object_point_itr); - }); - - // Add debug markers - { - const auto [color_r, color_g, color_b, color_a] = debug_colors; - addMarker( - this->get_clock()->now(), path, ego_polys, objects_from_point_clusters, - closest_object_point, color_r, color_g, color_b, color_a, debug_ns, debug_markers); - } - // check collision using rss distance - return (closest_object_point.has_value()) - ? hasCollision(current_v, closest_object_point.value()) - : false; - }; - - // step3. make function to check collision with ego path created with sensor data - const auto has_collision_ego = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { - if (!use_imu_path_) return false; - const double current_w = angular_velocity_ptr_->z; - constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; - const std::string ns = "ego"; - const auto ego_path = generateEgoPath(current_v, current_w); - - return check_collision(ego_path, debug_color, ns, filtered_objects); - }; - - // step4. make function to check collision with predicted trajectory from control module - const auto has_collision_predicted = - [&](pcl::PointCloud::Ptr filtered_objects) -> bool { - if (!use_predicted_trajectory_ || !predicted_traj_ptr_) return false; - const auto predicted_traj_ptr = predicted_traj_ptr_; - const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr); - - if (!predicted_path_opt) return false; - constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; - const std::string ns = "predicted"; - const auto & predicted_path = predicted_path_opt.value(); - - return check_collision(predicted_path, debug_color, ns, filtered_objects); - }; - - // Data of filtered point cloud - pcl::PointCloud::Ptr filtered_objects(new PointCloud); - // evaluate if there is a collision for both paths - const bool has_collision = - has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects); - - // Debug print - if (!filtered_objects->empty() && publish_debug_pointcloud_) { - const auto filtered_objects_ros_pointcloud_ptr_ = std::make_shared(); - pcl::toROSMsg(*filtered_objects, *filtered_objects_ros_pointcloud_ptr_); - pub_obstacle_pointcloud_->publish(*filtered_objects_ros_pointcloud_ptr_); - } - return has_collision; -} - -bool AEB::hasCollision(const double current_v, const ObjectData & closest_object) -{ - const double & obj_v = closest_object.velocity; - const double & t = t_response_; - const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - - obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; - if (closest_object.distance_to_object < rss_dist) { - // collision happens - ObjectData collision_data = closest_object; - collision_data.rss = rss_dist; - collision_data.distance_to_object = closest_object.distance_to_object; - collision_data_keeper_.setCollisionData(collision_data); - return true; - } - return false; -} - -Path AEB::generateEgoPath(const double curr_v, const double curr_w) -{ - Path path; - double curr_x = 0.0; - double curr_y = 0.0; - double curr_yaw = 0.0; - geometry_msgs::msg::Pose ini_pose; - ini_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); - ini_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); - path.push_back(ini_pose); - - if (curr_v < 0.1) { - // if current velocity is too small, assume it stops at the same point - return path; - } - - constexpr double epsilon = 1e-6; - const double & dt = imu_prediction_time_interval_; - const double & horizon = imu_prediction_time_horizon_; - for (double t = 0.0; t < horizon + epsilon; t += dt) { - curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; - curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; - curr_yaw = curr_yaw + curr_w * dt; - geometry_msgs::msg::Pose current_pose; - current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); - if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { - continue; - } - path.push_back(current_pose); - } - - // If path is shorter than minimum path length - while (motion_utils::calcArcLength(path) < min_generated_path_length_) { - curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; - curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; - curr_yaw = curr_yaw + curr_w * dt; - geometry_msgs::msg::Pose current_pose; - current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); - if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { - continue; - } - path.push_back(current_pose); - } - return path; -} - -std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) -{ - if (predicted_traj.points.empty()) { - return std::nullopt; - } - - geometry_msgs::msg::TransformStamped transform_stamped{}; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", predicted_traj.header.frame_id, predicted_traj.header.stamp, - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); - return std::nullopt; - } - - // create path - Path path; - path.resize(predicted_traj.points.size()); - for (size_t i = 0; i < predicted_traj.points.size(); ++i) { - geometry_msgs::msg::Pose map_pose; - tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); - path.at(i) = map_pose; - - if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { - break; - } - } - return path; -} - -std::vector AEB::generatePathFootprint( - const Path & path, const double extra_width_margin) -{ - std::vector polygons; - for (size_t i = 0; i < path.size() - 1; ++i) { - polygons.push_back( - createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); - } - return polygons; -} - -void AEB::createObjectDataUsingPointCloudClusters( - const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, - std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr) -{ - // check if the predicted path has valid number of points - if (ego_path.size() < 2 || ego_polys.empty() || obstacle_points_ptr->empty()) { - return; - } - - // eliminate noisy points by only considering points belonging to clusters of at least a certain - // size - const std::vector cluster_indices = std::invoke([&]() { - std::vector cluster_idx; - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); - tree->setInputCloud(obstacle_points_ptr); - pcl::EuclideanClusterExtraction ec; - ec.setClusterTolerance(cluster_tolerance_); - ec.setMinClusterSize(minimum_cluster_size_); - ec.setMaxClusterSize(maximum_cluster_size_); - ec.setSearchMethod(tree); - ec.setInputCloud(obstacle_points_ptr); - ec.extract(cluster_idx); - return cluster_idx; - }); - - PointCloud::Ptr points_belonging_to_cluster_hulls(new PointCloud); - for (const auto & indices : cluster_indices) { - PointCloud::Ptr cluster(new PointCloud); - for (const auto & index : indices.indices) { - cluster->push_back((*obstacle_points_ptr)[index]); - } - // Make a 2d convex hull for the objects - pcl::ConvexHull hull; - hull.setDimension(2); - hull.setInputCloud(cluster); - std::vector polygons; - PointCloud::Ptr surface_hull(new pcl::PointCloud); - hull.reconstruct(*surface_hull, polygons); - for (const auto & p : *surface_hull) { - points_belonging_to_cluster_hulls->push_back(p); - } - } - - // select points inside the ego footprint path - const auto current_p = tier4_autoware_utils::createPoint( - ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z); - - for (const auto & p : *points_belonging_to_cluster_hulls) { - const auto obj_position = tier4_autoware_utils::createPoint(p.x, p.y, p.z); - const double dist_ego_to_object = - motion_utils::calcSignedArcLength(ego_path, current_p, obj_position) - - vehicle_info_.max_longitudinal_offset_m; - // objects behind ego are ignored - if (dist_ego_to_object < 0.0) continue; - - ObjectData obj; - obj.stamp = stamp; - obj.position = obj_position; - obj.velocity = 0.0; - obj.distance_to_object = dist_ego_to_object; - - const Point2d obj_point(p.x, p.y); - for (const auto & ego_poly : ego_polys) { - if (bg::within(obj_point, ego_poly)) { - objects.push_back(obj); - break; - } - } - } -} - -void AEB::cropPointCloudWithEgoFootprintPath( - const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects) -{ - PointCloud::Ptr full_points_ptr(new PointCloud); - pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr); - // Create a Point cloud with the points of the ego footprint - PointCloud::Ptr path_polygon_points(new PointCloud); - std::for_each(ego_polys.begin(), ego_polys.end(), [&](const auto & poly) { - std::for_each(poly.outer().begin(), poly.outer().end(), [&](const auto & p) { - pcl::PointXYZ point(p.x(), p.y(), 0.0); - path_polygon_points->push_back(point); - }); - }); - // Make a surface hull with the ego footprint to filter out points - pcl::ConvexHull hull; - hull.setDimension(2); - hull.setInputCloud(path_polygon_points); - std::vector polygons; - pcl::PointCloud::Ptr surface_hull(new pcl::PointCloud); - hull.reconstruct(*surface_hull, polygons); - // Filter out points outside of the path's convex hull - pcl::CropHull path_polygon_hull_filter; - path_polygon_hull_filter.setDim(2); - path_polygon_hull_filter.setInputCloud(full_points_ptr); - path_polygon_hull_filter.setHullIndices(polygons); - path_polygon_hull_filter.setHullCloud(surface_hull); - path_polygon_hull_filter.filter(*filtered_objects); - filtered_objects->header.stamp = full_points_ptr->header.stamp; -} - -void AEB::addMarker( - const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, - const std::vector & objects, const std::optional & closest_object, - const double color_r, const double color_g, const double color_b, const double color_a, - const std::string & ns, MarkerArray & debug_markers) -{ - auto path_marker = tier4_autoware_utils::createDefaultMarker( - "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, - tier4_autoware_utils::createMarkerScale(0.2, 0.2, 0.2), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); - path_marker.points.resize(path.size()); - for (size_t i = 0; i < path.size(); ++i) { - path_marker.points.at(i) = path.at(i).position; - } - debug_markers.markers.push_back(path_marker); - - auto polygon_marker = tier4_autoware_utils::createDefaultMarker( - "base_link", current_time, ns + "_polygon", 0, Marker::LINE_LIST, - tier4_autoware_utils::createMarkerScale(0.03, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); - for (const auto & poly : polygons) { - for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { - const auto & boost_cp = poly.outer().at(dp_idx); - const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); - const auto curr_point = tier4_autoware_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); - const auto next_point = tier4_autoware_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); - polygon_marker.points.push_back(curr_point); - polygon_marker.points.push_back(next_point); - } - } - debug_markers.markers.push_back(polygon_marker); - - auto object_data_marker = tier4_autoware_utils::createDefaultMarker( - "base_link", current_time, ns + "_objects", 0, Marker::SPHERE_LIST, - tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5), - tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); - for (const auto & e : objects) { - object_data_marker.points.push_back(e.position); - } - debug_markers.markers.push_back(object_data_marker); - - // Visualize planner type text - if (closest_object.has_value()) { - const auto & obj = closest_object.value(); - const auto color = tier4_autoware_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999); - auto closest_object_velocity_marker_array = tier4_autoware_utils::createDefaultMarker( - "base_link", obj.stamp, ns + "_closest_object_velocity", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 0.7), color); - closest_object_velocity_marker_array.pose.position = obj.position; - const auto ego_velocity = current_velocity_ptr_->longitudinal_velocity; - closest_object_velocity_marker_array.text = - "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n"; - closest_object_velocity_marker_array.text += - "Object relative velocity to ego: " + std::to_string(obj.velocity - ego_velocity) + " [m/s]"; - debug_markers.markers.push_back(closest_object_velocity_marker_array); - } -} - -void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) -{ - auto point_marker = tier4_autoware_utils::createDefaultMarker( - "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, - tier4_autoware_utils::createMarkerScale(0.3, 0.3, 0.3), - tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); - point_marker.pose.position = data.position; - debug_markers.markers.push_back(point_marker); -} - -} // namespace autoware::motion::control::autonomous_emergency_braking - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::autonomous_emergency_braking::AEB) diff --git a/control/autoware_autonomous_emergency_braking/CMakeLists.txt b/control/autoware_autonomous_emergency_braking/CMakeLists.txt new file mode 100644 index 0000000000000..7f38e4387b452 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_autonomous_emergency_braking) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED) + +include_directories( + include + SYSTEM + ${PCL_INCLUDE_DIRS} +) + +set(AEB_NODE ${PROJECT_NAME}_node) +ament_auto_add_library(${AEB_NODE} SHARED + src/node.cpp +) + +rclcpp_components_register_node(${AEB_NODE} + PLUGIN "autoware::motion::control::autonomous_emergency_braking::AEB" + EXECUTABLE ${PROJECT_NAME} +) + +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/autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md similarity index 100% rename from control/autonomous_emergency_braking/README.md rename to control/autoware_autonomous_emergency_braking/README.md diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml similarity index 100% rename from control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml rename to control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml diff --git a/control/autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg similarity index 100% rename from control/autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg rename to control/autoware_autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg diff --git a/control/autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg similarity index 100% rename from control/autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg rename to control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg diff --git a/control/autonomous_emergency_braking/image/range.drawio.svg b/control/autoware_autonomous_emergency_braking/image/range.drawio.svg similarity index 100% rename from control/autonomous_emergency_braking/image/range.drawio.svg rename to control/autoware_autonomous_emergency_braking/image/range.drawio.svg diff --git a/control/autonomous_emergency_braking/image/rss_check.drawio.svg b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg similarity index 100% rename from control/autonomous_emergency_braking/image/rss_check.drawio.svg rename to control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg diff --git a/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp new file mode 100644 index 0000000000000..3e72d72cb9946 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp @@ -0,0 +1,343 @@ +// 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 AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#define AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__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 +#include +#include +#include +#include +#include +namespace autoware::motion::control::autonomous_emergency_braking +{ + +using autoware_planning_msgs::msg::Trajectory; +using autoware_system_msgs::msg::AutowareState; +using autoware_vehicle_msgs::msg::VelocityReport; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::Imu; +using sensor_msgs::msg::PointCloud2; +using PointCloud = pcl::PointCloud; +using autoware::vehicle_info_utils::VehicleInfo; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +using Path = std::vector; +using Vector3 = geometry_msgs::msg::Vector3; +struct ObjectData +{ + rclcpp::Time stamp; + geometry_msgs::msg::Point position; + double velocity{0.0}; + double rss{0.0}; + double distance_to_object{0.0}; +}; + +class CollisionDataKeeper +{ +public: + explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } + + void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time) + { + collision_keep_time_ = collision_keep_time; + previous_obstacle_keep_time_ = previous_obstacle_keep_time; + } + + std::pair getTimeout() + { + return {collision_keep_time_, previous_obstacle_keep_time_}; + } + + bool checkObjectDataExpired(std::optional & data, const double timeout) + { + if (!data.has_value()) return true; + const auto now = clock_->now(); + const auto & prev_obj = data.value(); + const auto & data_time_stamp = prev_obj.stamp; + if ((now - data_time_stamp).nanoseconds() * 1e-9 > timeout) { + data = std::nullopt; + return true; + } + return false; + } + + bool checkCollisionExpired() + { + return this->checkObjectDataExpired(closest_object_, collision_keep_time_); + } + + bool checkPreviousObjectDataExpired() + { + return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); + } + + ObjectData get() const + { + return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); + } + + ObjectData getPreviousObjectData() const + { + return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); + } + + void setCollisionData(const ObjectData & data) + { + closest_object_ = std::make_optional(data); + } + + void setPreviousObjectData(const ObjectData & data) + { + prev_closest_object_ = std::make_optional(data); + } + + void resetVelocityHistory() { obstacle_velocity_history_.clear(); } + + void updateVelocityHistory( + const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp) + { + // remove old msg from deque + const auto now = clock_->now(); + obstacle_velocity_history_.erase( + std::remove_if( + obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(), + [&](const auto & velocity_time_pair) { + const auto & vel_time = velocity_time_pair.second; + return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_); + }), + obstacle_velocity_history_.end()); + obstacle_velocity_history_.emplace_back( + std::make_pair(current_object_velocity, current_object_velocity_time_stamp)); + } + + std::optional getMedianObstacleVelocity() const + { + if (obstacle_velocity_history_.empty()) return std::nullopt; + std::vector raw_velocities; + for (const auto & vel_time_pair : obstacle_velocity_history_) { + raw_velocities.emplace_back(vel_time_pair.first); + } + + const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1 + : (raw_velocities.size()) / 2.0; + const size_t med2 = (raw_velocities.size()) / 2.0; + std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end()); + const double vel1 = raw_velocities.at(med1); + std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end()); + const double vel2 = raw_velocities.at(med2); + return (vel1 + vel2) / 2.0; + } + + std::optional calcObjectSpeedFromHistory( + const ObjectData & closest_object, const Path & path, const double current_ego_speed) + { + if (this->checkPreviousObjectDataExpired()) { + this->setPreviousObjectData(closest_object); + this->resetVelocityHistory(); + return std::nullopt; + } + + const auto estimated_velocity_opt = std::invoke([&]() -> std::optional { + const auto & prev_object = this->getPreviousObjectData(); + const double p_dt = + (closest_object.stamp.nanoseconds() - prev_object.stamp.nanoseconds()) * 1e-9; + if (p_dt < std::numeric_limits::epsilon()) return std::nullopt; + const auto & nearest_collision_point = closest_object.position; + const auto & prev_collision_point = prev_object.position; + + const double p_dx = nearest_collision_point.x - prev_collision_point.x; + const double p_dy = nearest_collision_point.y - prev_collision_point.y; + const double p_dist = std::hypot(p_dx, p_dy); + const double p_yaw = std::atan2(p_dy, p_dx); + const double p_vel = p_dist / p_dt; + + const auto nearest_idx = motion_utils::findNearestIndex(path, nearest_collision_point); + const auto & nearest_path_pose = path.at(nearest_idx); + const auto & traj_yaw = tf2::getYaw(nearest_path_pose.orientation); + const auto estimated_velocity = p_vel * std::cos(p_yaw - traj_yaw) + current_ego_speed; + + // Current RSS distance calculation does not account for negative velocities + return (estimated_velocity > 0.0) ? estimated_velocity : 0.0; + }); + + if (!estimated_velocity_opt.has_value()) { + return std::nullopt; + } + + const auto & estimated_velocity = estimated_velocity_opt.value(); + this->setPreviousObjectData(closest_object); + this->updateVelocityHistory(estimated_velocity, closest_object.stamp); + return this->getMedianObstacleVelocity(); + } + +private: + std::optional prev_closest_object_{std::nullopt}; + std::optional closest_object_{std::nullopt}; + double collision_keep_time_{0.0}; + double previous_obstacle_keep_time_{0.0}; + + std::deque> obstacle_velocity_history_; + rclcpp::Clock::SharedPtr clock_; +}; + +static rclcpp::SensorDataQoS SingleDepthSensorQoS() +{ + rclcpp::SensorDataQoS qos; + qos.get_rmw_qos_profile().depth = 1; + return qos; +} + +class AEB : public rclcpp::Node +{ +public: + explicit AEB(const rclcpp::NodeOptions & node_options); + + // subscriber + tier4_autoware_utils::InterProcessPollingSubscriber sub_point_cloud_{ + this, "~/input/pointcloud", SingleDepthSensorQoS()}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_velocity_{ + this, "~/input/velocity"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_traj_{ + this, "~/input/predicted_trajectory"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_autoware_state_{ + this, "/autoware/state"}; + // publisher + rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; + rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug + + // timer + rclcpp::TimerBase::SharedPtr timer_; + + // callback + void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); + void onImu(const Imu::ConstSharedPtr input_msg); + void onTimer(); + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + + bool fetchLatestData(); + + // main function + void onCheckCollision(DiagnosticStatusWrapper & stat); + bool checkCollision(MarkerArray & debug_markers); + bool hasCollision(const double current_v, const ObjectData & closest_object); + + Path generateEgoPath(const double curr_v, const double curr_w); + std::optional generateEgoPath(const Trajectory & predicted_traj); + std::vector generatePathFootprint(const Path & path, const double extra_width_margin); + + void createObjectDataUsingPointCloudClusters( + const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + std::vector & objects, + const pcl::PointCloud::Ptr obstacle_points_ptr); + + void cropPointCloudWithEgoFootprintPath( + const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects); + + void createObjectDataUsingPointCloudClusters( + const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + std::vector & objects); + void cropPointCloudWithEgoFootprintPath(const std::vector & ego_polys); + + void addMarker( + const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, + const std::vector & objects, const std::optional & closest_object, + const double color_r, const double color_g, const double color_b, const double color_a, + const std::string & ns, MarkerArray & debug_markers); + + void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); + + std::optional calcObjectSpeedFromHistory( + const ObjectData & closest_object, const Path & path, const double current_ego_speed); + + PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; + VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; + Vector3::SharedPtr angular_velocity_ptr_{nullptr}; + Trajectory::ConstSharedPtr predicted_traj_ptr_{nullptr}; + AutowareState::ConstSharedPtr autoware_state_{nullptr}; + + tf2_ros::Buffer tf_buffer_{get_clock()}; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + + // vehicle info + VehicleInfo vehicle_info_; + + // diag + Updater updater_{this}; + + // member variables + bool publish_debug_pointcloud_; + bool use_predicted_trajectory_; + bool use_imu_path_; + double path_footprint_extra_margin_; + double detection_range_min_height_; + double detection_range_max_height_margin_; + double voxel_grid_x_; + double voxel_grid_y_; + double voxel_grid_z_; + double min_generated_path_length_; + double expand_width_; + double longitudinal_offset_; + double t_response_; + double a_ego_min_; + double a_obj_min_; + double cluster_tolerance_; + int minimum_cluster_size_; + int maximum_cluster_size_; + double imu_prediction_time_horizon_; + double imu_prediction_time_interval_; + double mpc_prediction_time_horizon_; + double mpc_prediction_time_interval_; + CollisionDataKeeper collision_data_keeper_; + // Parameter callback + OnSetParametersCallbackHandle::SharedPtr set_param_res_; +}; +} // namespace autoware::motion::control::autonomous_emergency_braking + +#endif // AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ diff --git a/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml new file mode 100644 index 0000000000000..75b1a9dcf822d --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml new file mode 100644 index 0000000000000..0404eec7ca308 --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -0,0 +1,46 @@ + + + + autoware_autonomous_emergency_braking + 0.1.0 + Autonomous Emergency Braking package as a ROS 2 node + Takamasa Horibe + Tomoya Kimura + Mamoru Sobue + Daniel Sanchez + + Apache License 2.0 + + ament_cmake + autoware_cmake + + autoware_control_msgs + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_info_utils + autoware_vehicle_msgs + diagnostic_updater + geometry_msgs + motion_utils + nav_msgs + pcl_conversions + pcl_ros + pointcloud_preprocessor + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp new file mode 100644 index 0000000000000..d01855bc8ab5d --- /dev/null +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -0,0 +1,742 @@ +// 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 "autoware_autonomous_emergency_braking/node.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +namespace autoware::motion::control::autonomous_emergency_braking +{ +using diagnostic_msgs::msg::DiagnosticStatus; +namespace bg = boost::geometry; + +void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) +{ + Point2d point; + point.x() = geom_point.x; + point.y() = geom_point.y; + + bg::append(polygon.outer(), point); +} + +Polygon2d createPolygon( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware::vehicle_info_utils::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; + + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(base_pose, longitudinal_offset, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(base_pose, -rear_overhang, width, 0.0).position); + + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, width, 0.0).position); + appendPointToPolygon( + polygon, + tier4_autoware_utils::calcOffsetPose(next_pose, longitudinal_offset, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, -width, 0.0).position); + appendPointToPolygon( + polygon, tier4_autoware_utils::calcOffsetPose(next_pose, -rear_overhang, width, 0.0).position); + + polygon = tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); + + Polygon2d hull_polygon; + bg::convex_hull(polygon, hull_polygon); + + return hull_polygon; +} + +AEB::AEB(const rclcpp::NodeOptions & node_options) +: Node("AEB", node_options), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), + collision_data_keeper_(this->get_clock()) +{ + // Publisher + { + pub_obstacle_pointcloud_ = + this->create_publisher("~/debug/obstacle_pointcloud", 1); + debug_ego_path_publisher_ = this->create_publisher("~/debug/markers", 1); + } + // Diagnostics + { + updater_.setHardwareID("autonomous_emergency_braking"); + updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision); + } + // parameter + publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); + use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); + use_imu_path_ = declare_parameter("use_imu_path"); + path_footprint_extra_margin_ = declare_parameter("path_footprint_extra_margin"); + detection_range_min_height_ = declare_parameter("detection_range_min_height"); + detection_range_max_height_margin_ = + declare_parameter("detection_range_max_height_margin"); + voxel_grid_x_ = declare_parameter("voxel_grid_x"); + voxel_grid_y_ = declare_parameter("voxel_grid_y"); + voxel_grid_z_ = declare_parameter("voxel_grid_z"); + min_generated_path_length_ = declare_parameter("min_generated_path_length"); + expand_width_ = declare_parameter("expand_width"); + longitudinal_offset_ = declare_parameter("longitudinal_offset"); + t_response_ = declare_parameter("t_response"); + a_ego_min_ = declare_parameter("a_ego_min"); + a_obj_min_ = declare_parameter("a_obj_min"); + + cluster_tolerance_ = declare_parameter("cluster_tolerance"); + minimum_cluster_size_ = declare_parameter("minimum_cluster_size"); + maximum_cluster_size_ = declare_parameter("maximum_cluster_size"); + + imu_prediction_time_horizon_ = declare_parameter("imu_prediction_time_horizon"); + imu_prediction_time_interval_ = declare_parameter("imu_prediction_time_interval"); + mpc_prediction_time_horizon_ = declare_parameter("mpc_prediction_time_horizon"); + mpc_prediction_time_interval_ = declare_parameter("mpc_prediction_time_interval"); + + { // Object history data keeper setup + const auto previous_obstacle_keep_time = + declare_parameter("previous_obstacle_keep_time"); + const auto collision_keeping_sec = declare_parameter("collision_keeping_sec"); + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); + } + + // Parameter Callback + set_param_res_ = + add_on_set_parameters_callback(std::bind(&AEB::onParameter, this, std::placeholders::_1)); + + // start time + const double aeb_hz = declare_parameter("aeb_hz"); + const auto period_ns = rclcpp::Rate(aeb_hz).period(); + timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns, std::bind(&AEB::onTimer, this)); +} + +rcl_interfaces::msg::SetParametersResult AEB::onParameter( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); + updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); + updateParam(parameters, "use_imu_path", use_imu_path_); + updateParam(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_); + updateParam(parameters, "detection_range_min_height", detection_range_min_height_); + updateParam( + parameters, "detection_range_max_height_margin", detection_range_max_height_margin_); + updateParam(parameters, "voxel_grid_x", voxel_grid_x_); + updateParam(parameters, "voxel_grid_y", voxel_grid_y_); + updateParam(parameters, "voxel_grid_z", voxel_grid_z_); + updateParam(parameters, "min_generated_path_length", min_generated_path_length_); + updateParam(parameters, "expand_width", expand_width_); + updateParam(parameters, "longitudinal_offset", longitudinal_offset_); + updateParam(parameters, "t_response", t_response_); + updateParam(parameters, "a_ego_min", a_ego_min_); + updateParam(parameters, "a_obj_min", a_obj_min_); + + updateParam(parameters, "cluster_tolerance", cluster_tolerance_); + updateParam(parameters, "minimum_cluster_size", minimum_cluster_size_); + updateParam(parameters, "maximum_cluster_size", maximum_cluster_size_); + + updateParam(parameters, "imu_prediction_time_horizon", imu_prediction_time_horizon_); + updateParam(parameters, "imu_prediction_time_interval", imu_prediction_time_interval_); + updateParam(parameters, "mpc_prediction_time_horizon", mpc_prediction_time_horizon_); + updateParam(parameters, "mpc_prediction_time_interval", mpc_prediction_time_interval_); + + { // Object history data keeper setup + auto [previous_obstacle_keep_time, collision_keeping_sec] = collision_data_keeper_.getTimeout(); + updateParam(parameters, "previous_obstacle_keep_time", previous_obstacle_keep_time); + updateParam(parameters, "collision_keeping_sec", collision_keeping_sec); + collision_data_keeper_.setTimeout(collision_keeping_sec, previous_obstacle_keep_time); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +void AEB::onTimer() +{ + updater_.force_update(); +} + +void AEB::onImu(const Imu::ConstSharedPtr input_msg) +{ + // transform imu + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", input_msg->header.frame_id, input_msg->header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); + return; + } + + angular_velocity_ptr_ = std::make_shared(); + tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); +} + +void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) +{ + PointCloud::Ptr pointcloud_ptr(new PointCloud); + pcl::fromROSMsg(*input_msg, *pointcloud_ptr); + + if (input_msg->header.frame_id != "base_link") { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB]: Input point cloud frame is not base_link and it is " << input_msg->header.frame_id); + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", input_msg->header.frame_id, input_msg->header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), + "[AEB] Failed to look up transform from base_link to" << input_msg->header.frame_id); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *input_msg, transformed_points); + pcl::fromROSMsg(transformed_points, *pointcloud_ptr); + } + + // apply z-axis filter for removing False Positive points + PointCloud::Ptr height_filtered_pointcloud_ptr(new PointCloud); + pcl::PassThrough height_filter; + height_filter.setInputCloud(pointcloud_ptr); + height_filter.setFilterFieldName("z"); + height_filter.setFilterLimits( + detection_range_min_height_, + vehicle_info_.vehicle_height_m + detection_range_max_height_margin_); + height_filter.filter(*height_filtered_pointcloud_ptr); + + pcl::VoxelGrid filter; + PointCloud::Ptr no_height_filtered_pointcloud_ptr(new PointCloud); + filter.setInputCloud(height_filtered_pointcloud_ptr); + filter.setLeafSize(voxel_grid_x_, voxel_grid_y_, voxel_grid_z_); + filter.filter(*no_height_filtered_pointcloud_ptr); + + obstacle_ros_pointcloud_ptr_ = std::make_shared(); + + pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); + obstacle_ros_pointcloud_ptr_->header = input_msg->header; +} + +bool AEB::fetchLatestData() +{ + const auto missing = [this](const auto & name) { + RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name); + return false; + }; + + current_velocity_ptr_ = sub_velocity_.takeData(); + if (!current_velocity_ptr_) { + return missing("ego velocity"); + } + + const auto pointcloud_ptr = sub_point_cloud_.takeData(); + if (!pointcloud_ptr) { + return missing("object pointcloud message"); + } + onPointCloud(pointcloud_ptr); + if (!obstacle_ros_pointcloud_ptr_) { + return missing("object pointcloud"); + } + + const auto imu_ptr = sub_imu_.takeData(); + if (use_imu_path_) { + if (!imu_ptr) { + return missing("imu message"); + } + // imu_ptr is valid + onImu(imu_ptr); + } + if (use_imu_path_ && !angular_velocity_ptr_) { + return missing("imu"); + } + + predicted_traj_ptr_ = sub_predicted_traj_.takeData(); + if (use_predicted_trajectory_ && !predicted_traj_ptr_) { + return missing("control predicted trajectory"); + } + + autoware_state_ = sub_autoware_state_.takeData(); + if (!autoware_state_) { + return missing("autoware_state"); + } + + return true; +} + +void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) +{ + MarkerArray debug_markers; + checkCollision(debug_markers); + + if (!collision_data_keeper_.checkCollisionExpired()) { + const std::string error_msg = "[AEB]: Emergency Brake"; + const auto diag_level = DiagnosticStatus::ERROR; + stat.summary(diag_level, error_msg); + const auto & data = collision_data_keeper_.get(); + stat.addf("RSS", "%.2f", data.rss); + stat.addf("Distance", "%.2f", data.distance_to_object); + stat.addf("Object Speed", "%.2f", data.velocity); + addCollisionMarker(data, debug_markers); + } else { + const std::string error_msg = "[AEB]: No Collision"; + const auto diag_level = DiagnosticStatus::OK; + stat.summary(diag_level, error_msg); + } + + // publish debug markers + debug_ego_path_publisher_->publish(debug_markers); +} + +bool AEB::checkCollision(MarkerArray & debug_markers) +{ + using colorTuple = std::tuple; + + // step1. check data + if (!fetchLatestData()) { + return false; + } + + // if not driving, disable aeb + if (autoware_state_->state != AutowareState::DRIVING) { + return false; + } + + // step2. create velocity data check if the vehicle stops or not + const double current_v = current_velocity_ptr_->longitudinal_velocity; + if (current_v < 0.1) { + return false; + } + + auto check_collision = [&]( + const auto & path, const colorTuple & debug_colors, + const std::string & debug_ns, + pcl::PointCloud::Ptr filtered_objects) { + // Crop out Pointcloud using an extra wide ego path + const auto expanded_ego_polys = + generatePathFootprint(path, expand_width_ + path_footprint_extra_margin_); + cropPointCloudWithEgoFootprintPath(expanded_ego_polys, filtered_objects); + + // Check which points of the cropped point cloud are on the ego path, and get the closest one + std::vector objects_from_point_clusters; + const auto ego_polys = generatePathFootprint(path, expand_width_); + const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; + createObjectDataUsingPointCloudClusters( + path, ego_polys, current_time, objects_from_point_clusters, filtered_objects); + + // Get only the closest object and calculate its speed + const auto closest_object_point = std::invoke([&]() -> std::optional { + const auto closest_object_point_itr = std::min_element( + objects_from_point_clusters.begin(), objects_from_point_clusters.end(), + [](const auto & o1, const auto & o2) { + return o1.distance_to_object < o2.distance_to_object; + }); + + if (closest_object_point_itr == objects_from_point_clusters.end()) { + return std::nullopt; + } + const auto closest_object_speed = collision_data_keeper_.calcObjectSpeedFromHistory( + *closest_object_point_itr, path, current_v); + if (!closest_object_speed.has_value()) { + return std::nullopt; + } + closest_object_point_itr->velocity = closest_object_speed.value(); + return std::make_optional(*closest_object_point_itr); + }); + + // Add debug markers + { + const auto [color_r, color_g, color_b, color_a] = debug_colors; + addMarker( + this->get_clock()->now(), path, ego_polys, objects_from_point_clusters, + closest_object_point, color_r, color_g, color_b, color_a, debug_ns, debug_markers); + } + // check collision using rss distance + return (closest_object_point.has_value()) + ? hasCollision(current_v, closest_object_point.value()) + : false; + }; + + // step3. make function to check collision with ego path created with sensor data + const auto has_collision_ego = [&](pcl::PointCloud::Ptr filtered_objects) -> bool { + if (!use_imu_path_) return false; + const double current_w = angular_velocity_ptr_->z; + constexpr colorTuple debug_color = {0.0 / 256.0, 148.0 / 256.0, 205.0 / 256.0, 0.999}; + const std::string ns = "ego"; + const auto ego_path = generateEgoPath(current_v, current_w); + + return check_collision(ego_path, debug_color, ns, filtered_objects); + }; + + // step4. make function to check collision with predicted trajectory from control module + const auto has_collision_predicted = + [&](pcl::PointCloud::Ptr filtered_objects) -> bool { + if (!use_predicted_trajectory_ || !predicted_traj_ptr_) return false; + const auto predicted_traj_ptr = predicted_traj_ptr_; + const auto predicted_path_opt = generateEgoPath(*predicted_traj_ptr); + + if (!predicted_path_opt) return false; + constexpr colorTuple debug_color = {0.0 / 256.0, 100.0 / 256.0, 0.0 / 256.0, 0.999}; + const std::string ns = "predicted"; + const auto & predicted_path = predicted_path_opt.value(); + + return check_collision(predicted_path, debug_color, ns, filtered_objects); + }; + + // Data of filtered point cloud + pcl::PointCloud::Ptr filtered_objects(new PointCloud); + // evaluate if there is a collision for both paths + const bool has_collision = + has_collision_ego(filtered_objects) || has_collision_predicted(filtered_objects); + + // Debug print + if (!filtered_objects->empty() && publish_debug_pointcloud_) { + const auto filtered_objects_ros_pointcloud_ptr_ = std::make_shared(); + pcl::toROSMsg(*filtered_objects, *filtered_objects_ros_pointcloud_ptr_); + pub_obstacle_pointcloud_->publish(*filtered_objects_ros_pointcloud_ptr_); + } + return has_collision; +} + +bool AEB::hasCollision(const double current_v, const ObjectData & closest_object) +{ + const double & obj_v = closest_object.velocity; + const double & t = t_response_; + const double rss_dist = current_v * t + (current_v * current_v) / (2 * std::fabs(a_ego_min_)) - + obj_v * obj_v / (2 * std::fabs(a_obj_min_)) + longitudinal_offset_; + if (closest_object.distance_to_object < rss_dist) { + // collision happens + ObjectData collision_data = closest_object; + collision_data.rss = rss_dist; + collision_data.distance_to_object = closest_object.distance_to_object; + collision_data_keeper_.setCollisionData(collision_data); + return true; + } + return false; +} + +Path AEB::generateEgoPath(const double curr_v, const double curr_w) +{ + Path path; + double curr_x = 0.0; + double curr_y = 0.0; + double curr_yaw = 0.0; + geometry_msgs::msg::Pose ini_pose; + ini_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + ini_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + path.push_back(ini_pose); + + if (curr_v < 0.1) { + // if current velocity is too small, assume it stops at the same point + return path; + } + + constexpr double epsilon = 1e-6; + const double & dt = imu_prediction_time_interval_; + const double & horizon = imu_prediction_time_horizon_; + for (double t = 0.0; t < horizon + epsilon; t += dt) { + curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; + curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; + curr_yaw = curr_yaw + curr_w * dt; + geometry_msgs::msg::Pose current_pose; + current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + continue; + } + path.push_back(current_pose); + } + + // If path is shorter than minimum path length + while (motion_utils::calcArcLength(path) < min_generated_path_length_) { + curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; + curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; + curr_yaw = curr_yaw + curr_w * dt; + geometry_msgs::msg::Pose current_pose; + current_pose.position = tier4_autoware_utils::createPoint(curr_x, curr_y, 0.0); + current_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(curr_yaw); + if (tier4_autoware_utils::calcDistance2d(path.back(), current_pose) < 1e-3) { + continue; + } + path.push_back(current_pose); + } + return path; +} + +std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) +{ + if (predicted_traj.points.empty()) { + return std::nullopt; + } + + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_.lookupTransform( + "base_link", predicted_traj.header.frame_id, predicted_traj.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "[AEB] Failed to look up transform from base_link to map"); + return std::nullopt; + } + + // create path + Path path; + path.resize(predicted_traj.points.size()); + for (size_t i = 0; i < predicted_traj.points.size(); ++i) { + geometry_msgs::msg::Pose map_pose; + tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); + path.at(i) = map_pose; + + if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { + break; + } + } + return path; +} + +std::vector AEB::generatePathFootprint( + const Path & path, const double extra_width_margin) +{ + std::vector polygons; + for (size_t i = 0; i < path.size() - 1; ++i) { + polygons.push_back( + createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); + } + return polygons; +} + +void AEB::createObjectDataUsingPointCloudClusters( + const Path & ego_path, const std::vector & ego_polys, const rclcpp::Time & stamp, + std::vector & objects, const pcl::PointCloud::Ptr obstacle_points_ptr) +{ + // check if the predicted path has valid number of points + if (ego_path.size() < 2 || ego_polys.empty() || obstacle_points_ptr->empty()) { + return; + } + + // eliminate noisy points by only considering points belonging to clusters of at least a certain + // size + const std::vector cluster_indices = std::invoke([&]() { + std::vector cluster_idx; + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(obstacle_points_ptr); + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(cluster_tolerance_); + ec.setMinClusterSize(minimum_cluster_size_); + ec.setMaxClusterSize(maximum_cluster_size_); + ec.setSearchMethod(tree); + ec.setInputCloud(obstacle_points_ptr); + ec.extract(cluster_idx); + return cluster_idx; + }); + + PointCloud::Ptr points_belonging_to_cluster_hulls(new PointCloud); + for (const auto & indices : cluster_indices) { + PointCloud::Ptr cluster(new PointCloud); + for (const auto & index : indices.indices) { + cluster->push_back((*obstacle_points_ptr)[index]); + } + // Make a 2d convex hull for the objects + pcl::ConvexHull hull; + hull.setDimension(2); + hull.setInputCloud(cluster); + std::vector polygons; + PointCloud::Ptr surface_hull(new pcl::PointCloud); + hull.reconstruct(*surface_hull, polygons); + for (const auto & p : *surface_hull) { + points_belonging_to_cluster_hulls->push_back(p); + } + } + + // select points inside the ego footprint path + const auto current_p = tier4_autoware_utils::createPoint( + ego_path[0].position.x, ego_path[0].position.y, ego_path[0].position.z); + + for (const auto & p : *points_belonging_to_cluster_hulls) { + const auto obj_position = tier4_autoware_utils::createPoint(p.x, p.y, p.z); + const double dist_ego_to_object = + motion_utils::calcSignedArcLength(ego_path, current_p, obj_position) - + vehicle_info_.max_longitudinal_offset_m; + // objects behind ego are ignored + if (dist_ego_to_object < 0.0) continue; + + ObjectData obj; + obj.stamp = stamp; + obj.position = obj_position; + obj.velocity = 0.0; + obj.distance_to_object = dist_ego_to_object; + + const Point2d obj_point(p.x, p.y); + for (const auto & ego_poly : ego_polys) { + if (bg::within(obj_point, ego_poly)) { + objects.push_back(obj); + break; + } + } + } +} + +void AEB::cropPointCloudWithEgoFootprintPath( + const std::vector & ego_polys, pcl::PointCloud::Ptr filtered_objects) +{ + PointCloud::Ptr full_points_ptr(new PointCloud); + pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr); + // Create a Point cloud with the points of the ego footprint + PointCloud::Ptr path_polygon_points(new PointCloud); + std::for_each(ego_polys.begin(), ego_polys.end(), [&](const auto & poly) { + std::for_each(poly.outer().begin(), poly.outer().end(), [&](const auto & p) { + pcl::PointXYZ point(p.x(), p.y(), 0.0); + path_polygon_points->push_back(point); + }); + }); + // Make a surface hull with the ego footprint to filter out points + pcl::ConvexHull hull; + hull.setDimension(2); + hull.setInputCloud(path_polygon_points); + std::vector polygons; + pcl::PointCloud::Ptr surface_hull(new pcl::PointCloud); + hull.reconstruct(*surface_hull, polygons); + // Filter out points outside of the path's convex hull + pcl::CropHull path_polygon_hull_filter; + path_polygon_hull_filter.setDim(2); + path_polygon_hull_filter.setInputCloud(full_points_ptr); + path_polygon_hull_filter.setHullIndices(polygons); + path_polygon_hull_filter.setHullCloud(surface_hull); + path_polygon_hull_filter.filter(*filtered_objects); + filtered_objects->header.stamp = full_points_ptr->header.stamp; +} + +void AEB::addMarker( + const rclcpp::Time & current_time, const Path & path, const std::vector & polygons, + const std::vector & objects, const std::optional & closest_object, + const double color_r, const double color_g, const double color_b, const double color_a, + const std::string & ns, MarkerArray & debug_markers) +{ + auto path_marker = tier4_autoware_utils::createDefaultMarker( + "base_link", current_time, ns + "_path", 0L, Marker::LINE_STRIP, + tier4_autoware_utils::createMarkerScale(0.2, 0.2, 0.2), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + path_marker.points.resize(path.size()); + for (size_t i = 0; i < path.size(); ++i) { + path_marker.points.at(i) = path.at(i).position; + } + debug_markers.markers.push_back(path_marker); + + auto polygon_marker = tier4_autoware_utils::createDefaultMarker( + "base_link", current_time, ns + "_polygon", 0, Marker::LINE_LIST, + tier4_autoware_utils::createMarkerScale(0.03, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + for (const auto & poly : polygons) { + for (size_t dp_idx = 0; dp_idx < poly.outer().size(); ++dp_idx) { + const auto & boost_cp = poly.outer().at(dp_idx); + const auto & boost_np = poly.outer().at((dp_idx + 1) % poly.outer().size()); + const auto curr_point = tier4_autoware_utils::createPoint(boost_cp.x(), boost_cp.y(), 0.0); + const auto next_point = tier4_autoware_utils::createPoint(boost_np.x(), boost_np.y(), 0.0); + polygon_marker.points.push_back(curr_point); + polygon_marker.points.push_back(next_point); + } + } + debug_markers.markers.push_back(polygon_marker); + + auto object_data_marker = tier4_autoware_utils::createDefaultMarker( + "base_link", current_time, ns + "_objects", 0, Marker::SPHERE_LIST, + tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5), + tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a)); + for (const auto & e : objects) { + object_data_marker.points.push_back(e.position); + } + debug_markers.markers.push_back(object_data_marker); + + // Visualize planner type text + if (closest_object.has_value()) { + const auto & obj = closest_object.value(); + const auto color = tier4_autoware_utils::createMarkerColor(0.95, 0.95, 0.95, 0.999); + auto closest_object_velocity_marker_array = tier4_autoware_utils::createDefaultMarker( + "base_link", obj.stamp, ns + "_closest_object_velocity", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + tier4_autoware_utils::createMarkerScale(0.0, 0.0, 0.7), color); + closest_object_velocity_marker_array.pose.position = obj.position; + const auto ego_velocity = current_velocity_ptr_->longitudinal_velocity; + closest_object_velocity_marker_array.text = + "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n"; + closest_object_velocity_marker_array.text += + "Object relative velocity to ego: " + std::to_string(obj.velocity - ego_velocity) + " [m/s]"; + debug_markers.markers.push_back(closest_object_velocity_marker_array); + } +} + +void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers) +{ + auto point_marker = tier4_autoware_utils::createDefaultMarker( + "base_link", data.stamp, "collision_point", 0, Marker::SPHERE, + tier4_autoware_utils::createMarkerScale(0.3, 0.3, 0.3), + tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3)); + point_marker.pose.position = data.position; + debug_markers.markers.push_back(point_marker); +} + +} // namespace autoware::motion::control::autonomous_emergency_braking + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::motion::control::autonomous_emergency_braking::AEB) diff --git a/control/autoware_control_validator/CMakeLists.txt b/control/autoware_control_validator/CMakeLists.txt new file mode 100644 index 0000000000000..1a4119e47a723 --- /dev/null +++ b/control/autoware_control_validator/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(autoware_control_validator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_control_validator_helpers SHARED + src/utils.cpp + src/debug_marker.cpp +) + +# control validator +ament_auto_add_library(autoware_control_validator_component SHARED + include/autoware_control_validator/control_validator.hpp + src/control_validator.cpp +) +target_link_libraries(autoware_control_validator_component autoware_control_validator_helpers) +rclcpp_components_register_node(autoware_control_validator_component + PLUGIN "autoware::control_validator::ControlValidator" + EXECUTABLE autoware_control_validator_node +) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/ControlValidatorStatus.msg" + DEPENDENCIES builtin_interfaces +) + +# to use a message defined in the same package +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(autoware_control_validator_component + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(autoware_control_validator_component "${cpp_typesupport_target}") +endif() + +# if(BUILD_TESTING) +# endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/control/control_validator/README.md b/control/autoware_control_validator/README.md similarity index 100% rename from control/control_validator/README.md rename to control/autoware_control_validator/README.md diff --git a/control/control_validator/config/control_validator.param.yaml b/control/autoware_control_validator/config/control_validator.param.yaml similarity index 100% rename from control/control_validator/config/control_validator.param.yaml rename to control/autoware_control_validator/config/control_validator.param.yaml diff --git a/control/control_validator/image/control_validator.drawio.svg b/control/autoware_control_validator/image/control_validator.drawio.svg similarity index 100% rename from control/control_validator/image/control_validator.drawio.svg rename to control/autoware_control_validator/image/control_validator.drawio.svg diff --git a/control/control_validator/image/trajectory_deviation.drawio.svg b/control/autoware_control_validator/image/trajectory_deviation.drawio.svg similarity index 100% rename from control/control_validator/image/trajectory_deviation.drawio.svg rename to control/autoware_control_validator/image/trajectory_deviation.drawio.svg diff --git a/control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp b/control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp new file mode 100644 index 0000000000000..2e31de295f1f2 --- /dev/null +++ b/control/autoware_control_validator/include/autoware_control_validator/control_validator.hpp @@ -0,0 +1,101 @@ +// 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 AUTOWARE_CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ +#define AUTOWARE_CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ + +#include "autoware_control_validator/debug_marker.hpp" +#include "autoware_control_validator/msg/control_validator_status.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" + +#include +#include + +#include +#include +#include + +#include +#include + +namespace autoware::control_validator +{ +using autoware_control_validator::msg::ControlValidatorStatus; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using diagnostic_updater::DiagnosticStatusWrapper; +using diagnostic_updater::Updater; +using nav_msgs::msg::Odometry; + +struct ValidationParams +{ + double max_distance_deviation_threshold; +}; + +class ControlValidator : public rclcpp::Node +{ +public: + explicit ControlValidator(const rclcpp::NodeOptions & options); + + void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); + + bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory); + +private: + void setupDiag(); + + void setupParameters(); + + bool isDataReady(); + + void validate(const Trajectory & trajectory); + + void publishPredictedTrajectory(); + void publishDebugInfo(); + void displayStatus(); + + void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); + + rclcpp::Subscription::SharedPtr sub_predicted_traj_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + this, "~/input/kinematics"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_reference_traj_{ + this, "~/input/reference_trajectory"}; + rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_markers_; + + // system parameters + int diag_error_count_threshold_ = 0; + bool display_on_terminal_ = true; + + Updater diag_updater_{this}; + + ControlValidatorStatus validation_status_; + ValidationParams validation_params_; // for thresholds + + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + + bool isAllValid(const ControlValidatorStatus & status); + + Trajectory::ConstSharedPtr current_reference_trajectory_; + Trajectory::ConstSharedPtr current_predicted_trajectory_; + + Odometry::ConstSharedPtr current_kinematics_; + + std::shared_ptr debug_pose_publisher_; +}; +} // namespace autoware::control_validator + +#endif // AUTOWARE_CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ diff --git a/control/autoware_control_validator/include/autoware_control_validator/debug_marker.hpp b/control/autoware_control_validator/include/autoware_control_validator/debug_marker.hpp new file mode 100644 index 0000000000000..cde596ed7e1ab --- /dev/null +++ b/control/autoware_control_validator/include/autoware_control_validator/debug_marker.hpp @@ -0,0 +1,60 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ +#define AUTOWARE_CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +class ControlValidatorDebugMarkerPublisher +{ +public: + explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node); + + void pushPoseMarker( + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0); + void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); + void pushVirtualWall(const geometry_msgs::msg::Pose & pose); + void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); + void publish(); + + void clearMarkers(); + +private: + rclcpp::Node * node_; + visualization_msgs::msg::MarkerArray marker_array_; + visualization_msgs::msg::MarkerArray marker_array_virtual_wall_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr virtual_wall_pub_; + std::map marker_id_; + + int getMarkerId(const std::string & ns) + { + if (marker_id_.count(ns) == 0) { + marker_id_[ns] = 0.0; + } + return marker_id_[ns]++; + } +}; + +#endif // AUTOWARE_CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/control/autoware_control_validator/include/autoware_control_validator/utils.hpp b/control/autoware_control_validator/include/autoware_control_validator/utils.hpp new file mode 100644 index 0000000000000..69faa2fbe4abc --- /dev/null +++ b/control/autoware_control_validator/include/autoware_control_validator/utils.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 AUTOWARE_CONTROL_VALIDATOR__UTILS_HPP_ +#define AUTOWARE_CONTROL_VALIDATOR__UTILS_HPP_ + +#include +#include + +#include + +#include +#include +#include + +namespace autoware::control_validator +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using motion_utils::convertToTrajectory; +using motion_utils::convertToTrajectoryPointArray; +using TrajectoryPoints = std::vector; + +void shiftPose(Pose & pose, double longitudinal); + +void insertPointInPredictedTrajectory( + TrajectoryPoints & modified_trajectory, const geometry_msgs::msg::Pose & reference_pose, + const TrajectoryPoints & predicted_trajectory); + +TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory); + +bool removeFrontTrajectoryPoint( + const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, + const TrajectoryPoints & predicted_trajectory_points); + +Trajectory alignTrajectoryWithReferenceTrajectory( + const Trajectory & trajectory, const Trajectory & predicted_trajectory); + +double calcMaxLateralDistance( + const Trajectory & trajectory, const Trajectory & predicted_trajectory); +} // namespace autoware::control_validator + +#endif // AUTOWARE_CONTROL_VALIDATOR__UTILS_HPP_ diff --git a/control/control_validator/launch/control_validator.launch.xml b/control/autoware_control_validator/launch/control_validator.launch.xml similarity index 79% rename from control/control_validator/launch/control_validator.launch.xml rename to control/autoware_control_validator/launch/control_validator.launch.xml index 9727e06e60523..651398a17fc78 100644 --- a/control/control_validator/launch/control_validator.launch.xml +++ b/control/autoware_control_validator/launch/control_validator.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/control/control_validator/msg/ControlValidatorStatus.msg b/control/autoware_control_validator/msg/ControlValidatorStatus.msg similarity index 100% rename from control/control_validator/msg/ControlValidatorStatus.msg rename to control/autoware_control_validator/msg/ControlValidatorStatus.msg diff --git a/control/autoware_control_validator/package.xml b/control/autoware_control_validator/package.xml new file mode 100644 index 0000000000000..2059308ef9029 --- /dev/null +++ b/control/autoware_control_validator/package.xml @@ -0,0 +1,44 @@ + + + + autoware_control_validator + 0.1.0 + ros node for autoware_control_validator + Kyoichi Sugahara + Takamasa Horibe + Makoto Kurihara + Mamoru Sobue + Takayuki Murooka + + Apache License 2.0 + + Kyoichi Sugahara + Takamasa Horibe + Makoto Kurihara + + ament_cmake_auto + autoware_cmake + rosidl_default_generators + + autoware_planning_msgs + autoware_vehicle_info_utils + diagnostic_updater + geometry_msgs + motion_utils + nav_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp similarity index 86% rename from control/control_validator/src/control_validator.cpp rename to control/autoware_control_validator/src/control_validator.cpp index 5282e31fef898..7e4a86d42598e 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "control_validator/control_validator.hpp" +#include "autoware_control_validator/control_validator.hpp" -#include "control_validator/utils.hpp" +#include "autoware_control_validator/utils.hpp" #include #include #include -namespace control_validator +namespace autoware::control_validator { using diagnostic_msgs::msg::DiagnosticStatus; @@ -28,13 +28,6 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) : Node("control_validator", options) { using std::placeholders::_1; - - sub_kinematics_ = create_subscription( - "~/input/kinematics", 1, - [this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; }); - sub_reference_traj_ = create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&ControlValidator::onReferenceTrajectory, this, _1)); sub_predicted_traj_ = create_subscription( "~/input/predicted_trajectory", 1, std::bind(&ControlValidator::onPredictedTrajectory, this, _1)); @@ -61,7 +54,7 @@ void ControlValidator::setupParameters() } try { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); } catch (...) { RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value."); vehicle_info_.front_overhang_m = 0.5; @@ -116,21 +109,11 @@ bool ControlValidator::isDataReady() return true; } -void ControlValidator::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) -{ - if (msg->points.size() < 2) { - RCLCPP_ERROR(get_logger(), "planning trajectory size is invalid (%lu)", msg->points.size()); - return; - } - - current_reference_trajectory_ = msg; - - return; -} - void ControlValidator::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) { current_predicted_trajectory_ = msg; + current_reference_trajectory_ = sub_reference_traj_.takeData(); + current_kinematics_ = sub_kinematics_.takeData(); if (!isDataReady()) return; @@ -210,7 +193,7 @@ void ControlValidator::displayStatus() "predicted trajectory is too far from planning trajectory!!"); } -} // namespace control_validator +} // namespace autoware::control_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(control_validator::ControlValidator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::control_validator::ControlValidator) diff --git a/control/autoware_control_validator/src/debug_marker.cpp b/control/autoware_control_validator/src/debug_marker.cpp new file mode 100644 index 0000000000000..9ef2786c0aef6 --- /dev/null +++ b/control/autoware_control_validator/src/debug_marker.cpp @@ -0,0 +1,101 @@ +// 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 "autoware_control_validator/debug_marker.hpp" + +#include +#include + +#include +#include +#include + +using visualization_msgs::msg::Marker; + +ControlValidatorDebugMarkerPublisher::ControlValidatorDebugMarkerPublisher(rclcpp::Node * node) +: node_(node) +{ + debug_viz_pub_ = + node_->create_publisher("~/debug/marker", 1); + + virtual_wall_pub_ = + node_->create_publisher("~/virtual_wall", 1); +} + +void ControlValidatorDebugMarkerPublisher::clearMarkers() +{ + marker_array_.markers.clear(); + marker_array_virtual_wall_.markers.clear(); +} + +void ControlValidatorDebugMarkerPublisher::pushPoseMarker( + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) +{ + pushPoseMarker(p.pose, ns, id); +} + +void ControlValidatorDebugMarkerPublisher::pushPoseMarker( + const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) +{ + using tier4_autoware_utils::createMarkerColor; + + // append arrow marker + std_msgs::msg::ColorRGBA color; + if (id == 0) // Red + { + color = createMarkerColor(1.0, 0.0, 0.0, 0.999); + } + if (id == 1) // Green + { + color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + } + if (id == 2) // Blue + { + color = createMarkerColor(0.0, 0.0, 1.0, 0.999); + } + Marker marker = tier4_autoware_utils::createDefaultMarker( + "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, + tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3), color); + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.pose = pose; + + marker_array_.markers.push_back(marker); +} + +void ControlValidatorDebugMarkerPublisher::pushWarningMsg( + const geometry_msgs::msg::Pose & pose, const std::string & msg) +{ + visualization_msgs::msg::Marker marker = tier4_autoware_utils::createDefaultMarker( + "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, + tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), + tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(0.2); + marker.pose = pose; + marker.text = msg; + marker_array_virtual_wall_.markers.push_back(marker); +} + +void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs::msg::Pose & pose) +{ + const auto now = node_->get_clock()->now(); + const auto stop_wall_marker = + motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); + tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); +} + +void ControlValidatorDebugMarkerPublisher::publish() +{ + debug_viz_pub_->publish(marker_array_); + virtual_wall_pub_->publish(marker_array_virtual_wall_); +} diff --git a/control/autoware_control_validator/src/utils.cpp b/control/autoware_control_validator/src/utils.cpp new file mode 100644 index 0000000000000..e7cace049af6e --- /dev/null +++ b/control/autoware_control_validator/src/utils.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 "autoware_control_validator/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::control_validator +{ + +void shiftPose(Pose & pose, double longitudinal) +{ + const auto yaw = tf2::getYaw(pose.orientation); + pose.position.x += std::cos(yaw) * longitudinal; + pose.position.y += std::sin(yaw) * longitudinal; +} + +void insertPointInPredictedTrajectory( + TrajectoryPoints & modified_trajectory, const Pose & reference_pose, + const TrajectoryPoints & predicted_trajectory) +{ + const auto point_to_interpolate = + motion_utils::calcInterpolatedPoint(convertToTrajectory(predicted_trajectory), reference_pose); + modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate); +} + +TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) +{ + 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)); + return reversed_trajectory_points; +} + +bool removeFrontTrajectoryPoint( + const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, + const TrajectoryPoints & predicted_trajectory_points) +{ + bool predicted_trajectory_point_removed = false; + for (const auto & point : predicted_trajectory_points) { + if ( + motion_utils::calcLongitudinalOffsetToSegment(trajectory_points, 0, point.pose.position) < + 0.0) { + modified_trajectory_points.erase(modified_trajectory_points.begin()); + + predicted_trajectory_point_removed = true; + } else { + break; + } + } + + return predicted_trajectory_point_removed; +} + +Trajectory alignTrajectoryWithReferenceTrajectory( + const Trajectory & trajectory, const Trajectory & predicted_trajectory) +{ + const auto last_seg_length = motion_utils::calcSignedArcLength( + trajectory.points, trajectory.points.size() - 2, trajectory.points.size() - 1); + + // If no overlapping between trajectory and predicted_trajectory, return empty trajectory + // predicted_trajectory: p1------------------pN + // trajectory: t1------------------tN + // OR + // predicted_trajectory: p1------------------pN + // trajectory: t1------------------tN + const bool & is_p_n_before_t1 = + motion_utils::calcLongitudinalOffsetToSegment( + trajectory.points, 0, predicted_trajectory.points.back().pose.position) < 0.0; + const bool & is_p1_behind_t_n = motion_utils::calcLongitudinalOffsetToSegment( + trajectory.points, trajectory.points.size() - 2, + predicted_trajectory.points.front().pose.position) - + last_seg_length > + 0.0; + const bool is_no_overlapping = (is_p_n_before_t1 || is_p1_behind_t_n); + + if (is_no_overlapping) { + return Trajectory(); + } + + auto modified_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); + auto predicted_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); + auto trajectory_points = convertToTrajectoryPointArray(trajectory); + + // If first point of predicted_trajectory is in front of start of trajectory, erase points which + // are in front of trajectory start point and insert pNew along the predicted_trajectory + // predicted_trajectory: p1-----p2-----p3----//------pN + // trajectory: t1--------//------tN + // ↓ + // predicted_trajectory: pNew--p3----//------pN + // trajectory: t1--------//------tN + auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( + trajectory_points, modified_trajectory_points, predicted_trajectory_points); + + if (predicted_trajectory_point_removed) { + insertPointInPredictedTrajectory( + modified_trajectory_points, trajectory_points.front().pose, predicted_trajectory_points); + } + + // If last point of predicted_trajectory is behind of end of trajectory, erase points which are + // behind trajectory last point and insert pNew along the predicted_trajectory + // predicted_trajectory: p1-----//------pN-2-----pN-1-----pN + // trajectory: t1-----//-----tN-1--tN + // ↓ + // predicted_trajectory: p1-----//------pN-2-pNew + // trajectory: t1-----//-----tN-1--tN + + auto reversed_predicted_trajectory_points = reverseTrajectoryPoints(predicted_trajectory_points); + auto reversed_trajectory_points = reverseTrajectoryPoints(trajectory_points); + auto reversed_modified_trajectory_points = reverseTrajectoryPoints(modified_trajectory_points); + + auto reversed_predicted_trajectory_point_removed = removeFrontTrajectoryPoint( + reversed_trajectory_points, reversed_modified_trajectory_points, + reversed_predicted_trajectory_points); + + if (reversed_predicted_trajectory_point_removed) { + insertPointInPredictedTrajectory( + reversed_modified_trajectory_points, reversed_trajectory_points.front().pose, + reversed_predicted_trajectory_points); + } + + return convertToTrajectory(reverseTrajectoryPoints(reversed_modified_trajectory_points)); +} + +double calcMaxLateralDistance( + const Trajectory & reference_trajectory, const Trajectory & predicted_trajectory) +{ + const auto alined_predicted_trajectory = + alignTrajectoryWithReferenceTrajectory(reference_trajectory, predicted_trajectory); + double max_dist = 0; + for (const auto & point : alined_predicted_trajectory.points) { + const auto p0 = tier4_autoware_utils::getPoint(point); + // find nearest segment + const size_t nearest_segment_idx = + motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); + 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; + } + } + return max_dist; +} + +} // namespace autoware::control_validator diff --git a/control/autoware_external_cmd_selector/CMakeLists.txt b/control/autoware_external_cmd_selector/CMakeLists.txt new file mode 100644 index 0000000000000..4dccabcd85d8e --- /dev/null +++ b/control/autoware_external_cmd_selector/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_external_cmd_selector) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/autoware_external_cmd_selector/external_cmd_selector_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "autoware::external_cmd_selector::ExternalCmdSelector" + EXECUTABLE autoware_external_cmd_selector +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_external_cmd_selector/README.md b/control/autoware_external_cmd_selector/README.md new file mode 100644 index 0000000000000..0495004b68b84 --- /dev/null +++ b/control/autoware_external_cmd_selector/README.md @@ -0,0 +1,34 @@ +# autoware_external_cmd_selector + +## Purpose + +`autoware_external_cmd_selector` is the package to publish `external_control_cmd`, `gear_cmd`, `hazard_lights_cmd`, `heartbeat` and `turn_indicators_cmd`, according to the current mode, which is `remote` or `local`. + +The current mode is set via service, `remote` is remotely operated, `local` is to use the values calculated by Autoware. + +## Input / Output + +### Input topics + +| Name | Type | Description | +| ---------------------------------------------- | ---- | ------------------------------------------------------- | +| `/api/external/set/command/local/control` | TBD | Local. Calculated control value. | +| `/api/external/set/command/local/heartbeat` | TBD | Local. Heartbeat. | +| `/api/external/set/command/local/shift` | TBD | Local. Gear shift like drive, rear and etc. | +| `/api/external/set/command/local/turn_signal` | TBD | Local. Turn signal like left turn, right turn and etc. | +| `/api/external/set/command/remote/control` | TBD | Remote. Calculated control value. | +| `/api/external/set/command/remote/heartbeat` | TBD | Remote. Heartbeat. | +| `/api/external/set/command/remote/shift` | TBD | Remote. Gear shift like drive, rear and etc. | +| `/api/external/set/command/remote/turn_signal` | TBD | Remote. Turn signal like left turn, right turn and etc. | + +### Output topics + +| Name | Type | Description | +| ------------------------------------------------------ | ------------------------------------------------- | ----------------------------------------------- | +| `/control/external_cmd_selector/current_selector_mode` | TBD | Current selected mode, remote or local. | +| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Check if node is active or not. | +| `/external/selected/external_control_cmd` | TBD | Pass through control command with current mode. | +| `/external/selected/gear_cmd` | autoware_vehicle_msgs::msg::GearCommand | Pass through gear command with current mode. | +| `/external/selected/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | +| `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | +| `/external/selected/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | diff --git a/control/external_cmd_selector/config/external_cmd_selector.param.yaml b/control/autoware_external_cmd_selector/config/external_cmd_selector.param.yaml similarity index 100% rename from control/external_cmd_selector/config/external_cmd_selector.param.yaml rename to control/autoware_external_cmd_selector/config/external_cmd_selector.param.yaml diff --git a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp b/control/autoware_external_cmd_selector/include/autoware_external_cmd_selector/external_cmd_selector_node.hpp similarity index 93% rename from control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp rename to control/autoware_external_cmd_selector/include/autoware_external_cmd_selector/external_cmd_selector_node.hpp index 6a4fb897b57bc..1517ca83ad1f0 100644 --- a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp +++ b/control/autoware_external_cmd_selector/include/autoware_external_cmd_selector/external_cmd_selector_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ -#define EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +#ifndef AUTOWARE_EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +#define AUTOWARE_EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ #include #include @@ -30,7 +30,8 @@ #include #include - +namespace autoware::external_cmd_selector +{ class ExternalCmdSelector : public rclcpp::Node { public: @@ -101,5 +102,5 @@ class ExternalCmdSelector : public rclcpp::Node // Diagnostics Updater diagnostic_updater::Updater updater_{this}; }; - -#endif // EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +} // namespace autoware::external_cmd_selector +#endif // AUTOWARE_EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ diff --git a/control/external_cmd_selector/launch/external_cmd_selector.launch.py b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.py similarity index 97% rename from control/external_cmd_selector/launch/external_cmd_selector.launch.py rename to control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.py index 358cc135996f7..95673502e2b51 100644 --- a/control/external_cmd_selector/launch/external_cmd_selector.launch.py +++ b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.py @@ -32,8 +32,8 @@ def launch_setup(context, *args, **kwargs): external_cmd_selector_param = yaml.safe_load(f)["/**"]["ros__parameters"] component = ComposableNode( - package="external_cmd_selector", - plugin="ExternalCmdSelector", + package="autoware_external_cmd_selector", + plugin="autoware::external_cmd_selector::ExternalCmdSelector", name="external_cmd_selector", remappings=[ _create_mapping_tuple("service/select_external_command"), diff --git a/control/external_cmd_selector/launch/external_cmd_selector.launch.xml b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.xml similarity index 95% rename from control/external_cmd_selector/launch/external_cmd_selector.launch.xml rename to control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.xml index cb373802e5183..7e8b32ac68d8d 100644 --- a/control/external_cmd_selector/launch/external_cmd_selector.launch.xml +++ b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.xml @@ -26,7 +26,7 @@ - + diff --git a/control/autoware_external_cmd_selector/package.xml b/control/autoware_external_cmd_selector/package.xml new file mode 100644 index 0000000000000..19881f06c96e5 --- /dev/null +++ b/control/autoware_external_cmd_selector/package.xml @@ -0,0 +1,35 @@ + + + + autoware_external_cmd_selector + 0.1.0 + The autoware_external_cmd_selector package + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + Fumiya Watanabe + Takamasa Horibe + Satoshi Ota + Takayuki Murooka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_vehicle_msgs + diagnostic_updater + geometry_msgs + rclcpp + rclcpp_components + std_msgs + tier4_auto_msgs_converter + tier4_control_msgs + tier4_external_api_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp b/control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp similarity index 96% rename from control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp rename to control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp index 0039ec9cd1547..59a8caa5c1568 100644 --- a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp +++ b/control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "external_cmd_selector/external_cmd_selector_node.hpp" +#include "autoware_external_cmd_selector/external_cmd_selector_node.hpp" #include @@ -21,6 +21,8 @@ #include #include +namespace autoware::external_cmd_selector +{ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_options) : Node("external_cmd_selector", node_options) { @@ -202,6 +204,7 @@ ExternalCmdSelector::InternalHeartbeat ExternalCmdSelector::convert( { return command; } +} // namespace autoware::external_cmd_selector #include -RCLCPP_COMPONENTS_REGISTER_NODE(ExternalCmdSelector) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::external_cmd_selector::ExternalCmdSelector) diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt new file mode 100644 index 0000000000000..9c7d8b3ad475a --- /dev/null +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_joy_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_joy_controller_node SHARED + DIRECTORY src +) + +rclcpp_components_register_node(autoware_joy_controller_node + PLUGIN "autoware::joy_controller::AutowareJoyControllerNode" + EXECUTABLE autoware_joy_controller +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_joy_controller/README.md b/control/autoware_joy_controller/README.md new file mode 100644 index 0000000000000..a6919356f3dbc --- /dev/null +++ b/control/autoware_joy_controller/README.md @@ -0,0 +1,125 @@ +# autoware_joy_controller + +## Role + +`autoware_joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. + +## Usage + +### ROS 2 launch + +```bash +# With default config (ds4) +ros2 launch autoware_joy_controller joy_controller.launch.xml + +# Default config but select from the existing parameter files +ros2 launch autoware_joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox + +# Override the param file +ros2 launch autoware_joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml +``` + +## Input / Output + +### Input topics + +| Name | Type | Description | +| ------------------ | ----------------------- | --------------------------------- | +| `~/input/joy` | sensor_msgs::msg::Joy | joy controller command | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego vehicle odometry to get twist | + +### Output topics + +| Name | Type | Description | +| ----------------------------------- | --------------------------------------------------- | ---------------------------------------- | +| `~/output/control_command` | autoware_control_msgs::msg::Control | lateral and longitudinal control command | +| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | +| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command | +| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command | +| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) | +| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat | +| `~/output/vehicle_engage` | autoware_vehicle_msgs::msg::Engage | vehicle engage | + +## Parameters + +| Parameter | Type | Description | +| ------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ | +| `joy_type` | string | joy controller type (default: DS4) | +| `update_rate` | double | update rate to publish control commands | +| `accel_ratio` | double | ratio to calculate acceleration (commanded acceleration is ratio \* operation) | +| `brake_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | +| `steer_ratio` | double | ratio to calculate deceleration (commanded steer is ratio \* operation) | +| `steering_angle_velocity` | double | steering angle velocity for operation | +| `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `raw_control` | bool | skip input odometry if true | +| `velocity_gain` | double | ratio to calculate velocity by acceleration | +| `max_forward_velocity` | double | absolute max velocity to go forward | +| `max_backward_velocity` | double | absolute max velocity to go backward | +| `backward_accel_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | + +## P65 Joystick Key Map + +| Action | Button | +| -------------------- | --------------------- | +| Acceleration | R2 | +| Brake | L2 | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | L1 | +| Turn Signal Right | R1 | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | Select | +| Clear Emergency Stop | Start | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | PS | +| Vehicle Disengage | Right Trigger | + +## DS4 Joystick Key Map + +| Action | Button | +| -------------------- | -------------------------- | +| Acceleration | R2, ×, or Right Stick Up | +| Brake | L2, □, or Right Stick Down | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | L1 | +| Turn Signal Right | R1 | +| Clear Turn Signal | SHARE | +| Gate Mode | OPTIONS | +| Emergency Stop | PS | +| Clear Emergency Stop | PS | +| Autoware Engage | ○ | +| Autoware Disengage | ○ | +| Vehicle Engage | △ | +| Vehicle Disengage | △ | + +## XBOX Joystick Key Map + +| Action | Button | +| -------------------- | --------------------- | +| Acceleration | RT | +| Brake | LT | +| Steering | Left Stick Left Right | +| Shift up | Cursor Up | +| Shift down | Cursor Down | +| Shift Drive | Cursor Left | +| Shift Reverse | Cursor Right | +| Turn Signal Left | LB | +| Turn Signal Right | RB | +| Clear Turn Signal | A | +| Gate Mode | B | +| Emergency Stop | View | +| Clear Emergency Stop | Menu | +| Autoware Engage | X | +| Autoware Disengage | Y | +| Vehicle Engage | Left Stick Button | +| Vehicle Disengage | Right Stick Button | diff --git a/control/joy_controller/config/joy_controller_ds4.param.yaml b/control/autoware_joy_controller/config/joy_controller_ds4.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_ds4.param.yaml rename to control/autoware_joy_controller/config/joy_controller_ds4.param.yaml diff --git a/control/joy_controller/config/joy_controller_g29.param.yaml b/control/autoware_joy_controller/config/joy_controller_g29.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_g29.param.yaml rename to control/autoware_joy_controller/config/joy_controller_g29.param.yaml diff --git a/control/joy_controller/config/joy_controller_p65.param.yaml b/control/autoware_joy_controller/config/joy_controller_p65.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_p65.param.yaml rename to control/autoware_joy_controller/config/joy_controller_p65.param.yaml diff --git a/control/joy_controller/config/joy_controller_xbox.param.yaml b/control/autoware_joy_controller/config/joy_controller_xbox.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_xbox.param.yaml rename to control/autoware_joy_controller/config/joy_controller_xbox.param.yaml diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_controller.hpp similarity index 86% rename from control/joy_controller/include/joy_controller/joy_controller.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_controller.hpp index e88f7bcb3904e..88371455def62 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_controller.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONTROLLER_HPP_ -#define JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONTROLLER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" #include +#include #include #include @@ -36,7 +37,7 @@ #include #include -namespace joy_controller +namespace autoware::joy_controller { using GearShiftType = tier4_external_api_msgs::msg::GearShift::_data_type; using TurnSignalType = tier4_external_api_msgs::msg::TurnSignal::_data_type; @@ -66,19 +67,20 @@ class AutowareJoyControllerNode : public rclcpp::Node double backward_accel_ratio_; // CallbackGroups - rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; rclcpp::CallbackGroup::SharedPtr callback_group_services_; // Subscriber - rclcpp::Subscription::SharedPtr sub_joy_; - rclcpp::Subscription::SharedPtr sub_odom_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_joy_{ + this, "input/joy"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "input/odometry"}; rclcpp::Time last_joy_received_time_; std::shared_ptr joy_; geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; - void onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onJoy(); + void onOdometry(); // Publisher rclcpp::Publisher::SharedPtr pub_control_command_; @@ -118,6 +120,6 @@ class AutowareJoyControllerNode : public rclcpp::Node bool isDataReady(); void onTimer(); }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONTROLLER_HPP_ diff --git a/control/joy_controller/include/joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp similarity index 89% rename from control/joy_controller/include/joy_controller/joy_converter/ds4_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp index faa920d493478..7cfac1af8662b 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ -#define JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" #include -namespace joy_controller +namespace autoware::joy_controller { class DS4JoyConverter : public JoyConverterBase { @@ -90,6 +90,6 @@ class DS4JoyConverter : public JoyConverterBase bool reverse() const { return Share(); } }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/include/joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp similarity index 88% rename from control/joy_controller/include/joy_controller/joy_converter/g29_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp index 7ba062fe10d19..a36ad3a580287 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ -#define JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" -namespace joy_controller +namespace autoware::joy_controller { class G29JoyConverter : public JoyConverterBase { @@ -88,6 +88,6 @@ class G29JoyConverter : public JoyConverterBase bool reverse() const { return Share(); } }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/include/joy_controller/joy_converter/joy_converter_base.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp similarity index 82% rename from control/joy_controller/include/joy_controller/joy_converter/joy_converter_base.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp index 94587b88e22f5..98a8b799d71a9 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ -#define JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ #include #include -namespace joy_controller +namespace autoware::joy_controller { class JoyConverterBase { @@ -50,6 +50,6 @@ class JoyConverterBase virtual bool vehicle_engage() const = 0; virtual bool vehicle_disengage() const = 0; }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ diff --git a/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/p65_joy_converter.hpp similarity index 88% rename from control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/p65_joy_converter.hpp index e4cb822846fef..a6f51cb95b44d 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/p65_joy_converter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ -#define JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" #include -namespace joy_controller +namespace autoware::joy_controller { class P65JoyConverter : public JoyConverterBase { @@ -76,6 +76,6 @@ class P65JoyConverter : public JoyConverterBase const sensor_msgs::msg::Joy j_; }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/xbox_joy_converter.hpp similarity index 88% rename from control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/xbox_joy_converter.hpp index a009ee1e12975..776b0c98b7835 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/xbox_joy_converter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ -#define JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" #include -namespace joy_controller +namespace autoware::joy_controller { class XBOXJoyConverter : public JoyConverterBase { @@ -76,6 +76,6 @@ class XBOXJoyConverter : public JoyConverterBase const sensor_msgs::msg::Joy j_; }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/autoware_joy_controller/launch/joy_controller.launch.xml similarity index 89% rename from control/joy_controller/launch/joy_controller.launch.xml rename to control/autoware_joy_controller/launch/joy_controller.launch.xml index 5236077680d0d..02220f0026cea 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/joy_controller.launch.xml @@ -12,9 +12,9 @@ - + - + diff --git a/control/autoware_joy_controller/launch/joy_controller_param_selection.launch.xml b/control/autoware_joy_controller/launch/joy_controller_param_selection.launch.xml new file mode 100644 index 0000000000000..3d0aae9a45c4a --- /dev/null +++ b/control/autoware_joy_controller/launch/joy_controller_param_selection.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml new file mode 100644 index 0000000000000..10e33d5d6564a --- /dev/null +++ b/control/autoware_joy_controller/package.xml @@ -0,0 +1,39 @@ + + + + autoware_joy_controller + 0.1.0 + The autoware_joy_controller package + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + Fumiya Watanabe + Takamasa Horibe + Satoshi Ota + Takayuki Murooka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_control_msgs + autoware_vehicle_msgs + geometry_msgs + joy + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + std_srvs + tier4_api_utils + tier4_autoware_utils + tier4_control_msgs + tier4_external_api_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/autoware_joy_controller/schema/joy_controller.schema.json similarity index 97% rename from control/joy_controller/schema/joy_controller.schema.json rename to control/autoware_joy_controller/schema/joy_controller.schema.json index d4c6351324935..4c95057aec46d 100644 --- a/control/joy_controller/schema/joy_controller.schema.json +++ b/control/autoware_joy_controller/schema/joy_controller.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Joy Controller node", "type": "object", "definitions": { - "joy_controller": { + "autoware_joy_controller": { "type": "object", "properties": { "joy_type": { @@ -112,7 +112,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/joy_controller" + "$ref": "#/definitions/autoware_joy_controller" } }, "required": ["ros__parameters"], diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/autoware_joy_controller/src/joy_controller_node.cpp similarity index 91% rename from control/joy_controller/src/joy_controller/joy_controller_node.cpp rename to control/autoware_joy_controller/src/joy_controller_node.cpp index 39198825f9c4e..2491893a2806c 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/joy_controller_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "joy_controller/joy_controller.hpp" -#include "joy_controller/joy_converter/ds4_joy_converter.hpp" -#include "joy_controller/joy_converter/g29_joy_converter.hpp" -#include "joy_controller/joy_converter/p65_joy_converter.hpp" -#include "joy_controller/joy_converter/xbox_joy_converter.hpp" +#include "autoware_joy_controller/joy_controller.hpp" +#include "autoware_joy_controller/joy_converter/ds4_joy_converter.hpp" +#include "autoware_joy_controller/joy_converter/g29_joy_converter.hpp" +#include "autoware_joy_controller/joy_converter/p65_joy_converter.hpp" +#include "autoware_joy_controller/joy_converter/xbox_joy_converter.hpp" #include @@ -27,9 +27,9 @@ namespace { -using joy_controller::GateModeType; -using joy_controller::GearShiftType; -using joy_controller::TurnSignalType; +using autoware::joy_controller::GateModeType; +using autoware::joy_controller::GearShiftType; +using autoware::joy_controller::TurnSignalType; using GearShift = tier4_external_api_msgs::msg::GearShift; using TurnSignal = tier4_external_api_msgs::msg::TurnSignal; using GateMode = tier4_control_msgs::msg::GateMode; @@ -146,10 +146,11 @@ double calcMapping(const double input, const double sensitivity) } // namespace -namespace joy_controller +namespace autoware::joy_controller { -void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg) +void AutowareJoyControllerNode::onJoy() { + const auto msg = sub_joy_.takeData(); last_joy_received_time_ = msg->header.stamp; if (joy_type_ == "G29") { joy_ = std::make_shared(*msg); @@ -190,8 +191,13 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt } } -void AutowareJoyControllerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void AutowareJoyControllerNode::onOdometry() { + if (raw_control_) { + return; + } + + const auto msg = sub_odom_.takeData(); auto twist = std::make_shared(); twist->header = msg->header; twist->twist = msg->twist.twist; @@ -243,6 +249,9 @@ bool AutowareJoyControllerNode::isDataReady() void AutowareJoyControllerNode::onTimer() { + onOdometry(); + onJoy(); + if (!isDataReady()) { return; } @@ -450,7 +459,7 @@ void AutowareJoyControllerNode::initTimer(double period_s) } AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options) -: Node("joy_controller", node_options) +: Node("autoware_joy_controller", node_options) { // Parameter joy_type_ = declare_parameter("joy_type"); @@ -470,23 +479,11 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str()); // Callback Groups - callback_group_subscribers_ = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); callback_group_services_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscriber_option = rclcpp::SubscriptionOptions(); - subscriber_option.callback_group = callback_group_subscribers_; // Subscriber - sub_joy_ = this->create_subscription( - "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), - subscriber_option); - if (!raw_control_) { - sub_odom_ = this->create_subscription( - "input/odometry", 1, - std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), - subscriber_option); - } else { + if (raw_control_) { twist_ = std::make_shared(); } @@ -524,7 +521,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & // Timer initTimer(1.0 / update_rate_); } -} // namespace joy_controller +} // namespace autoware::joy_controller #include -RCLCPP_COMPONENTS_REGISTER_NODE(joy_controller::AutowareJoyControllerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::joy_controller::AutowareJoyControllerNode) diff --git a/control/autoware_lane_departure_checker/CMakeLists.txt b/control/autoware_lane_departure_checker/CMakeLists.txt new file mode 100644 index 0000000000000..d545e1cbb5ad1 --- /dev/null +++ b/control/autoware_lane_departure_checker/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_lane_departure_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +ament_auto_add_library(autoware_lane_departure_checker SHARED + src/lane_departure_checker_node/lane_departure_checker.cpp + src/lane_departure_checker_node/lane_departure_checker_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::lane_departure_checker::LaneDepartureCheckerNode" + EXECUTABLE lane_departure_checker_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/lane_departure_checker/README.md b/control/autoware_lane_departure_checker/README.md similarity index 100% rename from control/lane_departure_checker/README.md rename to control/autoware_lane_departure_checker/README.md diff --git a/control/lane_departure_checker/config/lane_departure_checker.param.yaml b/control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml similarity index 100% rename from control/lane_departure_checker/config/lane_departure_checker.param.yaml rename to control/autoware_lane_departure_checker/config/lane_departure_checker.param.yaml diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp similarity index 87% rename from control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp rename to control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp index 967cb65c8efa8..e29623bf69891 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ -#define LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#include #include #include #include -#include #include #include @@ -45,7 +45,7 @@ #include #include -namespace lane_departure_checker +namespace autoware::lane_departure_checker { using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::Trajectory; @@ -103,17 +103,17 @@ class LaneDepartureChecker public: Output update(const Input & input); - void setParam(const Param & param, const vehicle_info_util::VehicleInfo vehicle_info) + void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info) { param_ = param; - vehicle_info_ptr_ = std::make_shared(vehicle_info); + vehicle_info_ptr_ = std::make_shared(vehicle_info); } void setParam(const Param & param) { param_ = param; } - void setVehicleInfo(const vehicle_info_util::VehicleInfo vehicle_info) + void setVehicleInfo(const autoware::vehicle_info_utils::VehicleInfo vehicle_info) { - vehicle_info_ptr_ = std::make_shared(vehicle_info); + vehicle_info_ptr_ = std::make_shared(vehicle_info); } bool checkPathWillLeaveLane( @@ -137,7 +137,7 @@ class LaneDepartureChecker private: Param param_; - std::shared_ptr vehicle_info_ptr_; + std::shared_ptr vehicle_info_ptr_; static PoseDeviation calcTrajectoryDeviation( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, @@ -170,6 +170,6 @@ class LaneDepartureChecker const std::vector & vehicle_footprints, const SegmentRtree & uncrossable_segments); }; -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker -#endif // LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp similarity index 81% rename from control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp rename to control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp index 0f0e15d0a4f62..d37ef30d6b8e8 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ -#define LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ -#include "lane_departure_checker/lane_departure_checker.hpp" +#include "autoware_lane_departure_checker/lane_departure_checker.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include #include @@ -40,7 +41,7 @@ #include #include -namespace lane_departure_checker +namespace autoware::lane_departure_checker { using autoware_map_msgs::msg::LaneletMapBin; @@ -66,11 +67,16 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; - rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ + this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_route_{ + this, "~/input/route"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ + this, "~/input/reference_trajectory"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ + this, "~/input/predicted_trajectory"}; // Data Buffer nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; @@ -149,6 +155,6 @@ class LaneDepartureCheckerNode : public rclcpp::Node lanelet::Lanelets getRightOppositeLanelets(const lanelet::ConstLanelet & lanelet); }; -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker -#endif // LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ diff --git a/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp b/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp similarity index 89% rename from control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp rename to control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp index edc62cd6659fd..c85e37212b16f 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp +++ b/control/autoware_lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp @@ -25,12 +25,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#define AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ #include +#include #include -#include #include @@ -63,4 +63,4 @@ inline FootprintMargin calcFootprintMargin( return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale}; } -#endif // LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml b/control/autoware_lane_departure_checker/launch/lane_departure_checker.launch.xml similarity index 75% rename from control/lane_departure_checker/launch/lane_departure_checker.launch.xml rename to control/autoware_lane_departure_checker/launch/lane_departure_checker.launch.xml index 62799c1187651..b9f820f66690b 100644 --- a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml +++ b/control/autoware_lane_departure_checker/launch/lane_departure_checker.launch.xml @@ -4,12 +4,12 @@ - + - + - + diff --git a/control/autoware_lane_departure_checker/package.xml b/control/autoware_lane_departure_checker/package.xml new file mode 100644 index 0000000000000..7a08cd2907120 --- /dev/null +++ b/control/autoware_lane_departure_checker/package.xml @@ -0,0 +1,41 @@ + + + + autoware_lane_departure_checker + 0.1.0 + The autoware_lane_departure_checker package + Kyoichi Sugahara + Makoto Kurihara + Zulfaqar Azmi + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_map_msgs + autoware_planning_msgs + autoware_vehicle_info_utils + boost + diagnostic_updater + eigen + geometry_msgs + lanelet2_extension + motion_utils + nav_msgs + rclcpp + rclcpp_components + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/lane_departure_checker/schema/lane_departure_checker.json b/control/autoware_lane_departure_checker/schema/lane_departure_checker.json similarity index 100% rename from control/lane_departure_checker/schema/lane_departure_checker.json rename to control/autoware_lane_departure_checker/schema/lane_departure_checker.json diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp similarity index 98% rename from control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp rename to control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index eb5d674705aaa..5e90df6773b87 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lane_departure_checker/lane_departure_checker.hpp" +#include "autoware_lane_departure_checker/lane_departure_checker.hpp" -#include "lane_departure_checker/util/create_vehicle_footprint.hpp" +#include "autoware_lane_departure_checker/util/create_vehicle_footprint.hpp" #include #include @@ -96,7 +96,7 @@ lanelet::ConstLanelets getCandidateLanelets( } // namespace -namespace lane_departure_checker +namespace autoware::lane_departure_checker { Output LaneDepartureChecker::update(const Input & input) { @@ -460,4 +460,4 @@ bool LaneDepartureChecker::willCrossBoundary( return false; } -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp similarity index 92% rename from control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp rename to control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index c3fd1d314e371..f95ae0f4ac2ed 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lane_departure_checker/lane_departure_checker_node.hpp" +#include "autoware_lane_departure_checker/lane_departure_checker_node.hpp" #include #include @@ -120,7 +120,7 @@ void update_param( } // namespace -namespace lane_departure_checker +namespace autoware::lane_departure_checker { LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & options) : Node("lane_departure_checker_node", options) @@ -145,7 +145,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o declare_parameter>("boundary_types_to_detect"); // Vehicle Info - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_length_m_ = vehicle_info.vehicle_length_m; // Core Parameter @@ -169,22 +169,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o lane_departure_checker_ = std::make_unique(); lane_departure_checker_->setParam(param_, vehicle_info); - // Subscriber - sub_odom_ = this->create_subscription( - "~/input/odometry", 1, std::bind(&LaneDepartureCheckerNode::onOdometry, this, _1)); - sub_lanelet_map_bin_ = this->create_subscription( - "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(), - std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1)); - sub_route_ = this->create_subscription( - "~/input/route", rclcpp::QoS{1}.transient_local(), - std::bind(&LaneDepartureCheckerNode::onRoute, this, _1)); - sub_reference_trajectory_ = this->create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&LaneDepartureCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = this->create_subscription( - "~/input/predicted_trajectory", 1, - std::bind(&LaneDepartureCheckerNode::onPredictedTrajectory, this, _1)); - // Publisher // Nothing @@ -201,36 +185,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o this, get_clock(), period_ns, std::bind(&LaneDepartureCheckerNode::onTimer, this)); } -void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - current_odom_ = msg; -} - -void LaneDepartureCheckerNode::onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg) -{ - lanelet_map_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_); - - // get all shoulder lanes - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); - shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); -} - -void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) -{ - route_ = msg; -} - -void LaneDepartureCheckerNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) -{ - reference_trajectory_ = msg; -} - -void LaneDepartureCheckerNode::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) -{ - predicted_trajectory_ = msg; -} - bool LaneDepartureCheckerNode::isDataReady() { if (!current_odom_) { @@ -300,6 +254,22 @@ void LaneDepartureCheckerNode::onTimer() tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("Total"); + current_odom_ = sub_odom_.takeData(); + route_ = sub_route_.takeData(); + reference_trajectory_ = sub_reference_trajectory_.takeData(); + predicted_trajectory_ = sub_predicted_trajectory_.takeData(); + + const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeNewData(); + if (lanelet_map_bin_msg) { + lanelet_map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *lanelet_map_bin_msg, lanelet_map_, &traffic_rules_, &routing_graph_); + + // get all shoulder lanes + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); + } + if (!isDataReady()) { return; } @@ -759,7 +729,7 @@ lanelet::Lanelets LaneDepartureCheckerNode::getRightOppositeLanelets( return opposite_lanelets; } -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(lane_departure_checker::LaneDepartureCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lane_departure_checker::LaneDepartureCheckerNode) diff --git a/control/autoware_mpc_lateral_controller/CMakeLists.txt b/control/autoware_mpc_lateral_controller/CMakeLists.txt new file mode 100644 index 0000000000000..dff85d70419a1 --- /dev/null +++ b/control/autoware_mpc_lateral_controller/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_mpc_lateral_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(steering_offset_lib SHARED + src/steering_offset/steering_offset.cpp +) + +set(MPC_LAT_CON_LIB ${PROJECT_NAME}_lib) +ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED + src/mpc_lateral_controller.cpp + src/lowpass_filter.cpp + src/steering_predictor.cpp + src/mpc.cpp + src/mpc_trajectory.cpp + src/mpc_utils.cpp + src/qp_solver/qp_solver_osqp.cpp + src/qp_solver/qp_solver_unconstraint_fast.cpp + src/vehicle_model/vehicle_model_bicycle_dynamics.cpp + src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp + src/vehicle_model/vehicle_model_bicycle_kinematics.cpp + src/vehicle_model/vehicle_model_interface.cpp +) +target_link_libraries(${MPC_LAT_CON_LIB} steering_offset_lib) + +if(BUILD_TESTING) + set(TEST_LAT_SOURCES + test/test_mpc.cpp + test/test_mpc_utils.cpp + test/test_lowpass_filter.cpp + ) + set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller) + ament_add_ros_isolated_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) + target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${MPC_LAT_CON_LIB}) +endif() + +ament_auto_package(INSTALL_TO_SHARE + param +) diff --git a/control/autoware_mpc_lateral_controller/README.md b/control/autoware_mpc_lateral_controller/README.md new file mode 100644 index 0000000000000..6c5064a4aafe8 --- /dev/null +++ b/control/autoware_mpc_lateral_controller/README.md @@ -0,0 +1,266 @@ +# MPC Lateral Controller + +This is the design document for the lateral controller node +in the `autoware_trajectory_follower_node` package. + +## Purpose / Use cases + + + + +This node is used to general lateral control commands (steering angle and steering rate) +when following a path. + +## Design + + + + +The node uses an implementation of linear model predictive control (MPC) for accurate path tracking. +The MPC uses a model of the vehicle to simulate the trajectory resulting from the control command. +The optimization of the control command is formulated as a Quadratic Program (QP). + +Different vehicle models are implemented: + +- kinematics : bicycle kinematics model with steering 1st-order delay. +- kinematics_no_delay : bicycle kinematics model without steering delay. +- dynamics : bicycle dynamics model considering slip angle. + The kinematics model is being used by default. Please see the reference [1] for more details. + +For the optimization, a Quadratic Programming (QP) solver is used and two options are currently implemented: + + + +- unconstraint_fast : use least square method to solve unconstraint QP with eigen. +- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) + algorithm (for more details see the related papers at + the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section): + +### Filtering + +Filtering is required for good noise reduction. +A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is employed for processing the yaw and lateral errors, which are used as inputs for the MPC, as well as for refining the output steering angle. +Other filtering methods can be considered as long as the noise reduction performances are good +enough. +The moving average filter for example is not suited and can yield worse results than without any +filtering. + +## Assumptions / Known limits + + + +The tracking is not accurate if the first point of the reference trajectory is at or in front of the current ego pose. + +## Inputs / Outputs / API + + + + +### Inputs + +Set the following from the [controller_node](../autoware_trajectory_follower_node/README.md) + +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry +- `autoware_vehicle_msgs/SteeringReport`: current steering + +### Outputs + +Return LateralOutput which contains the following to the controller node + +- `autoware_control_msgs/Lateral` +- LateralSyncData + - steer angle convergence + +### MPC class + +The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm. +Once a vehicle model, a QP solver, and the reference trajectory to follow have been set +(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command +can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`. + +### Parameter description + +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. + +#### 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 (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] | [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 | +| :------------ | :----- | :------------------------------------------ | :------------ | +| cg_to_front_m | double | distance from baselink to the front axle[m] | 1.228 | +| cg_to_rear_m | double | distance from baselink to the rear axle [m] | 1.5618 | +| mass_fl | double | mass applied to front left tire [kg] | 600 | +| mass_fr | double | mass applied to front right tire [kg] | 600 | +| mass_rl | double | mass applied to rear left tire [kg] | 600 | +| mass_rr | double | mass applied to rear right tire [kg] | 600 | +| cf | double | front cornering power [N/rad] | 155494.663 | +| cr | double | rear cornering power [N/rad] | 155494.663 | + +### How to tune MPC parameters + +#### Set kinematics information + +First, it's important to set the appropriate parameters for vehicle kinematics. This includes parameters like `wheelbase`, which represents the distance between the front and rear wheels, and `max_steering_angle`, which indicates the maximum tire steering angle. These parameters should be set in the `vehicle_info.param.yaml`. + +#### Set dynamics information + +Next, you need to set the proper parameters for the dynamics model. These include the time constant `steering_tau` and time delay `steering_delay` for steering dynamics, and the maximum acceleration `mpc_acceleration_limit` and the time constant `mpc_velocity_time_constant` for velocity dynamics. + +#### Confirmation of the input information + +It's also important to make sure the input information is accurate. Information such as the velocity of the center of the rear wheel [m/s] and the steering angle of the tire [rad] is required. Please note that there have been frequent reports of performance degradation due to errors in input information. For instance, there are cases where the velocity of the vehicle is offset due to an unexpected difference in tire radius, or the tire angle cannot be accurately measured due to a deviation in the steering gear ratio or midpoint. It is suggested to compare information from multiple sensors (e.g., integrated vehicle speed and GNSS position, steering angle and IMU angular velocity), and ensure the input information for MPC is appropriate. + +#### MPC weight tuning + +Then, tune the weights of the MPC. One simple approach of tuning is to keep the weight for the lateral deviation (`weight_lat_error`) constant, and vary the input weight (`weight_steering_input`) while observing the trade-off between steering oscillation and control accuracy. + +Here, `weight_lat_error` acts to suppress the lateral error in path following, while `weight_steering_input` works to adjust the steering angle to a standard value determined by the path's curvature. When `weight_lat_error` is large, the steering moves significantly to improve accuracy, which can cause oscillations. On the other hand, when `weight_steering_input` is large, the steering doesn't respond much to tracking errors, providing stable driving but potentially reducing tracking accuracy. + +The steps are as follows: + +1. Set `weight_lat_error` = 0.1, `weight_steering_input` = 1.0 and other weights to 0. +2. If the vehicle oscillates when driving, set `weight_steering_input` larger. +3. If the tracking accuracy is low, set `weight_steering_input` smaller. + +If you want to adjust the effect only in the high-speed range, you can use `weight_steering_input_squared_vel`. This parameter corresponds to the steering weight in the high-speed range. + +#### Descriptions for weights + +- `weight_lat_error`: Reduce lateral tracking error. This acts like P gain in PID. +- `weight_heading_error`: Make a drive straight. This acts like D gain in PID. +- `weight_heading_error_squared_vel_coeff` : Make a drive straight in high speed range. +- `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. + +#### Other tips for tuning + +Here are some tips for adjusting other parameters: + +- In theory, increasing terminal weights, `weight_terminal_lat_error` and `weight_terminal_heading_error`, can enhance the tracking stability. This method sometimes proves effective. +- A larger `prediction_horizon` and a smaller `prediction_sampling_time` are efficient for tracking performance. However, these come at the cost of higher computational costs. +- If you want to modify the weight according to the trajectory curvature (for instance, when you're driving on a sharp curve and want a larger weight), use `mpc_low_curvature_thresh_curvature` and adjust `mpc_low_curvature_weight_**` weights. +- If you want to adjust the steering rate limit based on the vehicle speed and trajectory curvature, you can modify the values of `steer_rate_lim_dps_list_by_curvature`, `curvature_list_for_steer_rate_lim`, `steer_rate_lim_dps_list_by_velocity`, `velocity_list_for_steer_rate_lim`. By doing this, you can enforce the steering rate limit during high-speed driving or relax it while curving. +- In case your target curvature appears jagged, adjusting `curvature_smoothing` becomes critically important for accurate curvature calculations. A larger value yields a smooth curvature calculation which reduces noise but can cause delay in feedforward computation and potentially degrade performance. +- Adjusting the `steering_lpf_cutoff_hz` value can also be effective to forcefully reduce computational noise. This refers to the cutoff frequency in the second order Butterworth filter installed in the final layer. The smaller the cutoff frequency, the stronger the noise reduction, but it also induce operation delay. +- If the vehicle consistently deviates laterally from the trajectory, it's most often due to the offset of the steering sensor or self-position estimation. It's preferable to eliminate these biases before inputting into MPC, but it's also possible to remove this bias within MPC. To utilize this, set `enable_auto_steering_offset_removal` to true and activate the steering offset remover. The steering offset estimation logic works when driving at high speeds with the steering close to the center, applying offset removal. +- If the onset of steering in curves is late, it's often due to incorrect delay time and time constant in the steering model. Please recheck the values of `input_delay` and `vehicle_model_steer_tau`. Additionally, as a part of its debug information, MPC outputs the current steering angle assumed by the MPC model, so please check if that steering angle matches the actual one. + +## References / External links + + + +- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", + Robotics Institute, Carnegie Mellon University, February 2009. + +## Related issues + + diff --git a/control/mpc_lateral_controller/image/vehicle_error_kinematics.png b/control/autoware_mpc_lateral_controller/image/vehicle_error_kinematics.png similarity index 100% rename from control/mpc_lateral_controller/image/vehicle_error_kinematics.png rename to control/autoware_mpc_lateral_controller/image/vehicle_error_kinematics.png diff --git a/control/mpc_lateral_controller/image/vehicle_kinematics.png b/control/autoware_mpc_lateral_controller/image/vehicle_kinematics.png similarity index 100% rename from control/mpc_lateral_controller/image/vehicle_kinematics.png rename to control/autoware_mpc_lateral_controller/image/vehicle_kinematics.png diff --git a/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp new file mode 100644 index 0000000000000..73cef7cddda39 --- /dev/null +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp @@ -0,0 +1,105 @@ +// Copyright 2018-2021 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 AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ + +#include +#include +#include +#include + +namespace autoware::motion::control::mpc_lateral_controller +{ +/** + * @brief 2nd-order Butterworth Filter + * reference : S. Butterworth, "On the Theory of Filter Amplifier", Experimental wireless, 1930. + */ +class Butterworth2dFilter +{ +private: + double m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_y2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_u1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_u2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_a1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_a2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_b0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_b1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + double m_b2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time + +public: + /** + * @brief constructor with initialization + * @param [in] dt sampling time + * @param [in] f_cutoff_hz cutoff frequency [Hz] + */ + explicit Butterworth2dFilter(double dt = 0.01, double f_cutoff_hz = 5.0); + + /** + * @brief destructor + */ + ~Butterworth2dFilter(); + + /** + * @brief constructor + * @param [in] dt sampling time + * @param [in] f_cutoff_hz cutoff frequency [Hz] + */ + void initialize(const double & dt, const double & f_cutoff_hz); + + /** + * @brief filtering (call this function at each sampling time with input) + * @param [in] u scalar input for filter + * @return filtered scalar value + */ + double filter(const double & u); + + /** + * @brief filtering for time-series data + * @param [in] t time-series data for input vector + * @param [out] u object vector + */ + void filt_vector(const std::vector & t, std::vector & u) const; + + /** + * @brief filtering for time-series data from both forward-backward direction for zero phase delay + * @param [in] t time-series data for input vector + * @param [out] u object vector + */ + void filtfilt_vector( + const std::vector & t, + std::vector & u) const; // filtering forward and backward direction + + /** + * @brief get filter coefficients + * @param [out] coeffs coefficients of filter [a0, a1, a2, b0, b1, b2]. + */ + void getCoefficients(std::vector & coeffs) const; +}; + +/** + * @brief Move Average Filter + */ +namespace MoveAverageFilter +{ +/** + * @brief filtering vector + * @param [in] num index distance for moving average filter + * @param [out] u object vector + */ +bool filt_vector(const int num, std::vector & u); +} // namespace MoveAverageFilter +} // namespace autoware::motion::control::mpc_lateral_controller +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp similarity index 97% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp index 91b803dea36fa..03abae66e4986 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_HPP_ - -#include "mpc_lateral_controller/lowpass_filter.hpp" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" -#include "mpc_lateral_controller/steering_predictor.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ + +#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware_mpc_lateral_controller/steering_predictor.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" @@ -525,4 +525,4 @@ class MPC }; // class MPC } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp similarity index 94% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp index eb1d75f9508b3..50ab923f5b4d6 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ - -#include "mpc_lateral_controller/mpc.hpp" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" -#include "mpc_lateral_controller/steering_offset/steering_offset.hpp" +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ + +#include "autoware_mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" +#include "autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp" +#include "autoware_trajectory_follower_base/lateral_controller_base.hpp" #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower_base/lateral_controller_base.hpp" #include "autoware_control_msgs/msg/lateral.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -281,4 +281,4 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp similarity index 95% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp index eb624f39ae76b..4a0ae16bc9b1c 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -125,4 +125,4 @@ class MPCTrajectory } }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp similarity index 97% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp index 819d7fb89b8a7..e97a004438e4b 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -26,7 +26,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -230,4 +230,4 @@ void update_param( } // namespace MPCUtils } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp similarity index 89% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index ca30bd30e4dd1..6447437d6c274 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ #include @@ -51,4 +51,4 @@ class QPSolverInterface virtual double getObjVal() const { return 0.0; } }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp similarity index 88% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 2611f94da324f..8c81092960097 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" @@ -62,4 +62,4 @@ class QPSolverOSQP : public QPSolverInterface rclcpp::Logger logger_; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp similarity index 87% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp index ef337eaaa8528..15c8137b78890 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ -#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" namespace autoware::motion::control::mpc_lateral_controller { @@ -62,4 +62,4 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface private: }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp similarity index 84% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp index 494961ef679d2..707c1ce56a9a6 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ -#define MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ #include @@ -45,4 +45,4 @@ class SteeringOffsetEstimator double steering_offset_ = 0.0; }; -#endif // MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp similarity index 92% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp index 16e9b165fb1a5..0c5f04497d058 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ -#define MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ #include "rclcpp/rclcpp.hpp" @@ -81,4 +81,4 @@ class SteeringPredictor } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp similarity index 92% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 488a3c7ab8be2..20787b1f281b5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -44,10 +44,10 @@ * Tracking", Robotics Institute, Carnegie Mellon University, February 2009. */ -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -122,4 +122,4 @@ class DynamicsBicycleModel : public VehicleModelInterface double m_cr; //!< @brief rear cornering power [N/rad] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp similarity index 90% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index e2f66e95ab0e3..91d3ea826c7d3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -38,10 +38,10 @@ * */ -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -106,4 +106,4 @@ class KinematicsBicycleModel : public VehicleModelInterface double m_steer_tau; //!< @brief steering time constant for 1d-model [s] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp similarity index 87% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index b503ad8eb1d13..5731a2025d833 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -35,10 +35,10 @@ * */ -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -101,4 +101,6 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface double m_steer_lim; //!< @brief steering angle limit [rad] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +// clang-format off +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT +// clang-format on diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp similarity index 93% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index 68806ff5a00dc..9e2d14184d5e7 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" #include @@ -149,4 +149,4 @@ class VehicleModelInterface const MPCTrajectory & reference_trajectory, const double dt) const = 0; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/model_predictive_control_algorithm.md b/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md similarity index 100% rename from control/mpc_lateral_controller/model_predictive_control_algorithm.md rename to control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml new file mode 100644 index 0000000000000..1e250dd89ce3f --- /dev/null +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -0,0 +1,48 @@ + + + + autoware_mpc_lateral_controller + 1.0.0 + MPC-based lateral controller + + Takamasa Horibe + Takayuki Murooka + + Apache 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + autoware_control_msgs + autoware_planning_msgs + autoware_trajectory_follower_base + autoware_vehicle_info_utils + autoware_vehicle_msgs + diagnostic_msgs + diagnostic_updater + eigen + geometry_msgs + interpolation + motion_utils + osqp_interface + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml similarity index 100% rename from control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml rename to control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml diff --git a/control/mpc_lateral_controller/src/lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp similarity index 98% rename from control/mpc_lateral_controller/src/lowpass_filter.cpp rename to control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp index 8fbf3488c5a2e..e3bbe78e2428b 100644 --- a/control/mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/lowpass_filter.hpp" +#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" #include diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp similarity index 99% rename from control/mpc_lateral_controller/src/mpc.cpp rename to control/autoware_mpc_lateral_controller/src/mpc.cpp index 177c1e0854bfb..e6fa2ce3c0102 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp similarity index 97% rename from control/mpc_lateral_controller/src/mpc_lateral_controller.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 85d33a5e9f1c0..cf8a4ed5a251b 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc_lateral_controller.hpp" - +#include "autoware_mpc_lateral_controller/mpc_lateral_controller.hpp" + +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_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" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "tf2/utils.h" #include "tf2_ros/create_timer_ros.h" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -69,7 +69,7 @@ MpcLateralController::MpcLateralController(rclcpp::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 = autoware::vehicle_info_utils::VehicleInfoUtils(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; diff --git a/control/mpc_lateral_controller/src/mpc_trajectory.cpp b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp similarity index 97% rename from control/mpc_lateral_controller/src/mpc_trajectory.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp index b7fed942375e8..e84e6cf79a25a 100644 --- a/control/mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp similarity index 99% rename from control/mpc_lateral_controller/src/mpc_utils.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 01a81d9015b47..1a0062372eb22 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc_utils.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp similarity index 97% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp rename to control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index 7c76d67e75aa3..66981b0c5c6d4 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include #include diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp similarity index 93% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp rename to control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp index b0357138cd646..315e29d063aad 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp +++ b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include diff --git a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp b/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp similarity index 95% rename from control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp rename to control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp index 60d4dd7901394..6425d5da5e941 100644 --- a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp +++ b/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/steering_offset/steering_offset.hpp" +#include "autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp" #include #include diff --git a/control/mpc_lateral_controller/src/steering_predictor.cpp b/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp similarity index 98% rename from control/mpc_lateral_controller/src/steering_predictor.cpp rename to control/autoware_mpc_lateral_controller/src/steering_predictor.cpp index 48d8fa8c47a97..759a36a16de62 100644 --- a/control/mpc_lateral_controller/src/steering_predictor.cpp +++ b/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/steering_predictor.hpp" +#include "autoware_mpc_lateral_controller/steering_predictor.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index 2467b1f0c2311..d5272c48fcbdf 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index cd52dd4f79314..97bcafb161cb0 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 8f4510510aca9..d2857d8b42182 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp similarity index 94% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index f54a5e325bead..e1b17d3d96278 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp similarity index 98% rename from control/mpc_lateral_controller/test/test_lowpass_filter.cpp rename to control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp index c68513586847b..bfb695cdeee8a 100644 --- a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/lowpass_filter.hpp" #include diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp similarity index 97% rename from control/mpc_lateral_controller/test/test_mpc.cpp rename to control/autoware_mpc_lateral_controller/test/test_mpc.cpp index 7644fb28b0788..3fd4b010852c0 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/mpc.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" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" diff --git a/control/mpc_lateral_controller/test/test_mpc_utils.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp similarity index 95% rename from control/mpc_lateral_controller/test/test_mpc_utils.cpp rename to control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp index 51cc7d55e4560..b55435dffd024 100644 --- a/control/mpc_lateral_controller/test/test_mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/control/autoware_operation_mode_transition_manager/CMakeLists.txt b/control/autoware_operation_mode_transition_manager/CMakeLists.txt new file mode 100644 index 0000000000000..2ebd6ec153ed2 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_operation_mode_transition_manager) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(autoware_operation_mode_transition_manager_node SHARED + src/compatibility.cpp + src/data.cpp + src/node.cpp + src/state.cpp +) + +rclcpp_components_register_node(autoware_operation_mode_transition_manager_node + PLUGIN "autoware::operation_mode_transition_manager::OperationModeTransitionManager" + EXECUTABLE autoware_operation_mode_transition_manager_exe +) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/OperationModeTransitionManagerDebug.msg" + DEPENDENCIES builtin_interfaces +) + +# to use same package defined message +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(autoware_operation_mode_transition_manager_node + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(autoware_operation_mode_transition_manager_node "${cpp_typesupport_target}") +endif() + + +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/autoware_operation_mode_transition_manager/README.md b/control/autoware_operation_mode_transition_manager/README.md new file mode 100644 index 0000000000000..3a9680f42702b --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/README.md @@ -0,0 +1,138 @@ +# autoware_operation_mode_transition_manager + +## Purpose / Use cases + +This module is responsible for managing the different modes of operation for the Autoware system. The possible modes are: + +- `Autonomous`: the vehicle is fully controlled by the autonomous driving system +- `Local`: the vehicle is controlled by a physically connected control system such as a joy stick +- `Remote`: the vehicle is controlled by a remote controller +- `Stop`: the vehicle is stopped and there is no active control system. + +There is also an `In Transition` state that occurs during each mode transitions. During this state, the transition to the new operator is not yet complete, and the previous operator is still responsible for controlling the system until the transition is complete. Some actions may be restricted during the `In Transition` state, such as sudden braking or steering. (This is restricted by the `vehicle_cmd_gate`). + +### Features + +- Transit mode between `Autonomous`, `Local`, `Remote` and `Stop` based on the indication command. +- Check whether the each transition is available (safe or not). +- Limit some sudden motion control in `In Transition` mode (this is done with `vehicle_cmd_gate` feature). +- Check whether the transition is completed. + +- Transition between the `Autonomous`, `Local`, `Remote`, and `Stop` modes based on the indicated command. +- Determine whether each transition is safe to execute. +- Restrict certain sudden motion controls during the `In Transition` mode (using the `vehicle_cmd_gate` feature). +- Verify that the transition is complete. + +## Design + +A rough design of the relationship between `autoware_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 `autoware_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 + +### Inputs + +For the mode transition: + +- /system/operation_mode/change_autoware_control [`tier4_system_msgs/srv/ChangeAutowareControl`]: change operation mode to Autonomous +- /system/operation_mode/change_operation_mode [`tier4_system_msgs/srv/ChangeOperationMode`]: change operation mode + +For the transition availability/completion check: + +- /control/command/control_cmd [`autoware_control_msgs/msg/Control`]: vehicle control signal +- /localization/kinematic_state [`nav_msgs/msg/Odometry`]: ego vehicle state +- /planning/scenario_planning/trajectory [`autoware_planning_msgs/msg/Trajectory`]: planning trajectory +- /vehicle/status/control_mode [`autoware_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) +- /control/vehicle_cmd_gate/operation_mode [`autoware_adapi_v1_msgs/msg/OperationModeState`]: the operation mode in the `vehicle_cmd_gate`. (To be removed) + +For the backward compatibility (to be removed): + +- /api/autoware/get/engage [`autoware_vehicle_msgs/msg/Engage`] +- /control/current_gate_mode [`tier4_control_msgs/msg/GateMode`] +- /control/external_cmd_selector/current_selector_mode [`tier4_control_msgs/msg/ExternalCommandSelectorMode`] + +### Outputs + +- /system/operation_mode/state [`autoware_adapi_v1_msgs/msg/OperationModeState`]: to inform the current operation mode +- /control/autoware_operation_mode_transition_manager/debug_info [`autoware_operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug`]: detailed information about the operation mode transition + +- /control/gate_mode_cmd [`tier4_control_msgs/msg/GateMode`]: to change the `vehicle_cmd_gate` state to use its features (to be removed) +- /autoware/engage [`autoware_vehicle_msgs/msg/Engage`]: + +- /control/control_mode_request [`autoware_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) +- /control/external_cmd_selector/select_external_command [`tier4_control_msgs/srv/ExternalCommandSelect`]: + +## Parameters + +{{ json_to_markdown("control/autoware_operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json") }} + +| 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: + +| Name | Type | Description | Default value | +| :---------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `allow_autonomous_in_stopped` | `bool` | If true, autonomous transition is available when the vehicle is stopped even if other checks fail. | true | +| `dist_threshold` | `double` | the distance between the trajectory and ego vehicle must be within this distance for `Autonomous` transition. | 1.5 | +| `yaw_threshold` | `double` | the yaw angle between trajectory and ego vehicle must be within this threshold for `Autonomous` transition. | 0.524 | +| `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold for `Autonomous` transition. | 10.0 | +| `speed_lower_threshold` | `double` | the velocity deviation between the control command and ego vehicle must be within this threshold for `Autonomous` transition. | -10.0 | +| `acc_threshold` | `double` | the control command acceleration must be less than this threshold for `Autonomous` transition. | 1.5 | +| `lateral_acc_threshold` | `double` | the control command lateral acceleration must be less than this threshold for `Autonomous` transition. | 1.0 | +| `lateral_acc_diff_threshold` | `double` | the lateral acceleration deviation between the control command must be less than this threshold for `Autonomous` transition. | 0.5 | + +For `stable_check` related parameters: + +| Name | Type | Description | Default value | +| :---------------------- | :------- | :-------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `duration` | `double` | the stable condition must be satisfied for this duration to complete the transition. | 0.1 | +| `dist_threshold` | `double` | the distance between the trajectory and ego vehicle must be within this distance to complete `Autonomous` transition. | 1.5 | +| `yaw_threshold` | `double` | the yaw angle between trajectory and ego vehicle must be within this threshold to complete `Autonomous` transition. | 0.262 | +| `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. +- This node should be merged to the `vehicle_cmd_gate` due to its strong connection. diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/autoware_operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml similarity index 100% rename from control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml rename to control/autoware_operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml diff --git a/control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg b/control/autoware_operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg similarity index 100% rename from control/operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg rename to control/autoware_operation_mode_transition_manager/image/transition_detailed_structure.drawio.svg diff --git a/control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg b/control/autoware_operation_mode_transition_manager/image/transition_rough_structure.drawio.svg similarity index 100% rename from control/operation_mode_transition_manager/image/transition_rough_structure.drawio.svg rename to control/autoware_operation_mode_transition_manager/image/transition_rough_structure.drawio.svg diff --git a/control/autoware_operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml b/control/autoware_operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml new file mode 100644 index 0000000000000..700228b8afbc3 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg b/control/autoware_operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg similarity index 100% rename from control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg rename to control/autoware_operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg diff --git a/control/autoware_operation_mode_transition_manager/package.xml b/control/autoware_operation_mode_transition_manager/package.xml new file mode 100644 index 0000000000000..c93b38c537a73 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/package.xml @@ -0,0 +1,40 @@ + + autoware_operation_mode_transition_manager + 0.1.0 + The operation_mode_transition_manager package + Takamasa Horibe + Tomoya Kimura + Takayuki Murooka + Apache License 2.0 + + Takamasa Horibe + + autoware_cmake + rosidl_default_generators + + autoware_control_msgs + autoware_vehicle_info_utils + autoware_vehicle_msgs + component_interface_specs + component_interface_utils + geometry_msgs + motion_utils + rclcpp + rclcpp_components + std_srvs + tier4_autoware_utils + tier4_control_msgs + tier4_system_msgs + tier4_vehicle_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json b/control/autoware_operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json similarity index 100% rename from control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json rename to control/autoware_operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json diff --git a/control/operation_mode_transition_manager/src/compatibility.cpp b/control/autoware_operation_mode_transition_manager/src/compatibility.cpp similarity index 97% rename from control/operation_mode_transition_manager/src/compatibility.cpp rename to control/autoware_operation_mode_transition_manager/src/compatibility.cpp index 66a5fbd6b1b13..df22581dd361e 100644 --- a/control/operation_mode_transition_manager/src/compatibility.cpp +++ b/control/autoware_operation_mode_transition_manager/src/compatibility.cpp @@ -16,7 +16,7 @@ #include -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { Compatibility::Compatibility(rclcpp::Node * node) : node_(node) @@ -140,4 +140,4 @@ void Compatibility::set_mode(const OperationMode mode) } } -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/src/compatibility.hpp b/control/autoware_operation_mode_transition_manager/src/compatibility.hpp similarity index 95% rename from control/operation_mode_transition_manager/src/compatibility.hpp rename to control/autoware_operation_mode_transition_manager/src/compatibility.hpp index d9bde7cb22872..34d4c7a24dd30 100644 --- a/control/operation_mode_transition_manager/src/compatibility.hpp +++ b/control/autoware_operation_mode_transition_manager/src/compatibility.hpp @@ -22,7 +22,7 @@ #include #include -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { class Compatibility @@ -54,6 +54,6 @@ class Compatibility SelectorModeMsg::ConstSharedPtr selector_mode_; }; -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager #endif // COMPATIBILITY_HPP_ diff --git a/control/operation_mode_transition_manager/src/data.cpp b/control/autoware_operation_mode_transition_manager/src/data.cpp similarity index 94% rename from control/operation_mode_transition_manager/src/data.cpp rename to control/autoware_operation_mode_transition_manager/src/data.cpp index 8734b6d1c8e86..1fac496f3c71c 100644 --- a/control/operation_mode_transition_manager/src/data.cpp +++ b/control/autoware_operation_mode_transition_manager/src/data.cpp @@ -14,7 +14,7 @@ #include "data.hpp" -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { std::string toString(const std::optional mode) @@ -67,4 +67,4 @@ OperationModeValue toMsg(const OperationMode mode) return OperationModeState::UNKNOWN; } -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/src/data.hpp b/control/autoware_operation_mode_transition_manager/src/data.hpp similarity index 95% rename from control/operation_mode_transition_manager/src/data.hpp rename to control/autoware_operation_mode_transition_manager/src/data.hpp index 8ea9f8d2b99ee..63ccc90b3af85 100644 --- a/control/operation_mode_transition_manager/src/data.hpp +++ b/control/autoware_operation_mode_transition_manager/src/data.hpp @@ -27,7 +27,7 @@ #include #include -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { using ServiceResponse = autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response; @@ -81,6 +81,6 @@ std::string toString(const std::optional mode); std::optional toEnum(const ChangeOperationMode::Request & request); OperationModeValue toMsg(const OperationMode mode); -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager #endif // DATA_HPP_ diff --git a/control/autoware_operation_mode_transition_manager/src/node.cpp b/control/autoware_operation_mode_transition_manager/src/node.cpp new file mode 100644 index 0000000000000..a93c0b93741a1 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/src/node.cpp @@ -0,0 +1,296 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "node.hpp" + +#include + +namespace autoware::operation_mode_transition_manager +{ + +OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::NodeOptions & options) +: Node("autoware_operation_mode_transition_manager", options), compatibility_(this) +{ + cli_control_mode_ = create_client("control_mode_request"); + pub_debug_info_ = create_publisher("~/debug_info", 1); + + // component interface + { + const auto node = component_interface_utils::NodeAdaptor(this); + node.init_srv( + srv_autoware_control_, this, &OperationModeTransitionManager::onChangeAutowareControl); + node.init_srv( + srv_operation_mode_, this, &OperationModeTransitionManager::onChangeOperationMode); + node.init_pub(pub_operation_mode_); + } + + // timer + { + const auto period_ns = rclcpp::Rate(declare_parameter("frequency_hz")).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&OperationModeTransitionManager::onTimer, this)); + } + + // initialize state + current_mode_ = OperationMode::STOP; + transition_ = nullptr; + gate_operation_mode_.mode = OperationModeState::UNKNOWN; + gate_operation_mode_.is_in_transition = false; + control_mode_report_.mode = ControlModeReport::NO_COMMAND; + transition_timeout_ = declare_parameter("transition_timeout"); + { + // check `transition_timeout` value + const auto stable_duration = declare_parameter("stable_check.duration"); + const double TIMEOUT_MARGIN = 0.5; + if (transition_timeout_ < stable_duration + TIMEOUT_MARGIN) { + transition_timeout_ = stable_duration + TIMEOUT_MARGIN; + RCLCPP_WARN( + get_logger(), "`transition_timeout` must be somewhat larger than `stable_check.duration`"); + RCLCPP_WARN_STREAM(get_logger(), "transition_timeout is set to " << transition_timeout_); + } + } + + // modes + modes_[OperationMode::STOP] = std::make_unique(); + modes_[OperationMode::AUTONOMOUS] = std::make_unique(this); + modes_[OperationMode::LOCAL] = std::make_unique(); + modes_[OperationMode::REMOTE] = std::make_unique(); +} + +void OperationModeTransitionManager::onChangeAutowareControl( + const ChangeAutowareControlAPI::Service::Request::SharedPtr request, + const ChangeAutowareControlAPI::Service::Response::SharedPtr response) +{ + if (request->autoware_control) { + // Treat as a transition to the current operation mode. + changeOperationMode(std::nullopt); + } else { + // Allow mode transition to complete without canceling. + compatibility_transition_ = std::nullopt; + transition_.reset(); + changeControlMode(ControlModeCommand::Request::MANUAL); + } + response->status.success = true; +} + +void OperationModeTransitionManager::onChangeOperationMode( + const ChangeOperationModeAPI::Service::Request::SharedPtr request, + const ChangeOperationModeAPI::Service::Response::SharedPtr response) +{ + const auto mode = toEnum(*request); + if (!mode) { + throw component_interface_utils::ParameterError("The operation mode is invalid."); + } + changeOperationMode(mode.value()); + response->status.success = true; +} + +void OperationModeTransitionManager::changeControlMode(ControlModeCommandType mode) +{ + const auto callback = [this](rclcpp::Client::SharedFuture future) { + if (!future.get()->success) { + RCLCPP_WARN(get_logger(), "Autonomous mode change was rejected."); + if (transition_) { + transition_->is_engage_failed = true; + } + } + }; + + const auto request = std::make_shared(); + request->stamp = now(); + request->mode = mode; + cli_control_mode_->async_send_request(request, callback); +} + +void OperationModeTransitionManager::changeOperationMode(std::optional request_mode) +{ + // NOTE: If request_mode is nullopt, indicates to enable autoware control + + const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; + const bool request_control = request_mode ? false : true; + + if (current_mode_ == request_mode) { + throw component_interface_utils::NoEffectWarning("The mode is the same as the current."); + } + + if (current_control && request_control) { + throw component_interface_utils::NoEffectWarning("Autoware already controls the vehicle."); + } + + // TODO(Takagi, Isamu): Consider mode change request during transition. + if (transition_ && request_mode != OperationMode::STOP) { + throw component_interface_utils::ServiceException( + ServiceResponse::ERROR_IN_TRANSITION, "The mode transition is in progress."); + } + + // Enter transition mode if the vehicle is being or will be controlled by Autoware. + if (current_control || request_control) { + if (!available_mode_change_[request_mode.value_or(current_mode_)]) { + throw component_interface_utils::ServiceException( + ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); + } + if (request_control) { + transition_ = std::make_unique(now(), request_control, std::nullopt); + } else { + transition_ = std::make_unique(now(), request_control, current_mode_); + } + } + compatibility_transition_ = now(); + current_mode_ = request_mode.value_or(current_mode_); +} + +void OperationModeTransitionManager::cancelTransition() +{ + const auto & previous = transition_->previous; + if (previous) { + compatibility_transition_ = now(); + current_mode_ = previous.value(); + } else { + compatibility_transition_ = std::nullopt; + changeControlMode(ControlModeCommand::Request::MANUAL); + } + transition_.reset(); +} + +void OperationModeTransitionManager::processTransition() +{ + const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; + + // Check timeout. + if (transition_timeout_ < (now() - transition_->time).seconds()) { + return cancelTransition(); + } + + // Check engage failure. + if (transition_->is_engage_failed) { + return cancelTransition(); + } + + // Check override. + if (current_control) { + transition_->is_engage_completed = true; + } else { + if (transition_->is_engage_completed) { + return cancelTransition(); + } + } + + // Check reflection of mode change to the compatible interface. + if (current_mode_ != compatibility_.get_mode()) { + return; + } + + // Check completion when engaged, otherwise engage after the gate reflects transition. + if (current_control) { + if (modes_.at(current_mode_)->isModeChangeCompleted()) { + return transition_.reset(); + } + } else { + if (transition_->is_engage_requested && gate_operation_mode_.is_in_transition) { + transition_->is_engage_requested = false; + return changeControlMode(ControlModeCommand::Request::AUTONOMOUS); + } + } +} + +void OperationModeTransitionManager::onTimer() +{ + const auto control_mode_report_ptr = sub_control_mode_report_.takeData(); + if (!control_mode_report_ptr) { + return; + } + const auto gate_operation_mode_ptr = sub_gate_operation_mode_.takeData(); + if (!gate_operation_mode_ptr) { + return; + } + control_mode_report_ = *control_mode_report_ptr; + gate_operation_mode_ = *gate_operation_mode_ptr; + + for (const auto & [type, mode] : modes_) { + mode->update(current_mode_ == type && transition_); + } + + for (const auto & [type, mode] : modes_) { + available_mode_change_[type] = mode->isModeChangeAvailable(); + } + + // Check sync timeout to the compatible interface. + if (compatibility_transition_) { + if (compatibility_timeout_ < (now() - compatibility_transition_.value()).seconds()) { + compatibility_transition_ = std::nullopt; + } + } + + // Reflects the mode when changed by the compatible interface. + if (compatibility_transition_) { + compatibility_.set_mode(current_mode_); + } else { + current_mode_ = compatibility_.get_mode().value_or(current_mode_); + } + + // Reset sync timeout when it is completed. + if (compatibility_transition_) { + if (current_mode_ == compatibility_.get_mode()) { + compatibility_transition_ = std::nullopt; + } + } + + if (transition_) { + processTransition(); + } + + publishData(); +} + +void OperationModeTransitionManager::publishData() +{ + const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; + const auto time = now(); + + OperationModeStateAPI::Message state; + state.mode = toMsg(current_mode_); + state.is_autoware_control_enabled = current_control; + state.is_in_transition = transition_ ? true : false; + state.is_stop_mode_available = available_mode_change_[OperationMode::STOP]; + state.is_autonomous_mode_available = available_mode_change_[OperationMode::AUTONOMOUS]; + state.is_local_mode_available = available_mode_change_[OperationMode::LOCAL]; + state.is_remote_mode_available = available_mode_change_[OperationMode::REMOTE]; + + if (prev_state_ != state) { + prev_state_ = state; + state.stamp = time; + pub_operation_mode_->publish(state); + } + + const auto status_str = [&]() { + if (!current_control) return "DISENGAGE (autoware mode = " + toString(current_mode_) + ")"; + if (transition_) + return toString(current_mode_) + " (in transition from " + toString(transition_->previous) + + ")"; + return toString(current_mode_); + }(); + + ModeChangeBase::DebugInfo debug_info = modes_.at(OperationMode::AUTONOMOUS)->getDebugInfo(); + debug_info.stamp = time; + debug_info.status = status_str; + debug_info.in_autoware_control = current_control; + debug_info.in_transition = transition_ ? true : false; + pub_debug_info_->publish(debug_info); +} + +} // namespace autoware::operation_mode_transition_manager + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::operation_mode_transition_manager::OperationModeTransitionManager) diff --git a/control/autoware_operation_mode_transition_manager/src/node.hpp b/control/autoware_operation_mode_transition_manager/src/node.hpp new file mode 100644 index 0000000000000..0cacb083fc293 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/src/node.hpp @@ -0,0 +1,83 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NODE_HPP_ +#define NODE_HPP_ + +#include "compatibility.hpp" +#include "state.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace autoware::operation_mode_transition_manager +{ + +class OperationModeTransitionManager : public rclcpp::Node +{ +public: + explicit OperationModeTransitionManager(const rclcpp::NodeOptions & options); + +private: + using ChangeAutowareControlAPI = system_interface::ChangeAutowareControl; + using ChangeOperationModeAPI = system_interface::ChangeOperationMode; + using OperationModeStateAPI = system_interface::OperationModeState; + component_interface_utils::Service::SharedPtr srv_autoware_control_; + component_interface_utils::Service::SharedPtr srv_operation_mode_; + component_interface_utils::Publisher::SharedPtr pub_operation_mode_; + void onChangeAutowareControl( + const ChangeAutowareControlAPI::Service::Request::SharedPtr request, + const ChangeAutowareControlAPI::Service::Response::SharedPtr response); + void onChangeOperationMode( + const ChangeOperationModeAPI::Service::Request::SharedPtr request, + const ChangeOperationModeAPI::Service::Response::SharedPtr response); + + using ControlModeCommandType = ControlModeCommand::Request::_mode_type; + tier4_autoware_utils::InterProcessPollingSubscriber sub_control_mode_report_{ + this, "control_mode_report"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_gate_operation_mode_{ + this, "gate_operation_mode"}; + rclcpp::Client::SharedPtr cli_control_mode_; + rclcpp::Publisher::SharedPtr pub_debug_info_; + rclcpp::TimerBase::SharedPtr timer_; + void onTimer(); + void publishData(); + void changeControlMode(ControlModeCommandType mode); + void changeOperationMode(std::optional mode); + void cancelTransition(); + void processTransition(); + + double transition_timeout_; + OperationMode current_mode_; + std::unique_ptr transition_; + std::unordered_map> modes_; + std::unordered_map available_mode_change_; + OperationModeState gate_operation_mode_; + ControlModeReport control_mode_report_; + + std::optional prev_state_; + + static constexpr double compatibility_timeout_ = 1.0; + Compatibility compatibility_; + std::optional compatibility_transition_; +}; + +} // namespace autoware::operation_mode_transition_manager + +#endif // NODE_HPP_ diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/autoware_operation_mode_transition_manager/src/state.cpp similarity index 98% rename from control/operation_mode_transition_manager/src/state.cpp rename to control/autoware_operation_mode_transition_manager/src/state.cpp index 635ead2430677..4eaf0ddf8e764 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/autoware_operation_mode_transition_manager/src/state.cpp @@ -23,7 +23,7 @@ #include #include -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { using motion_utils::findNearestIndex; @@ -33,7 +33,7 @@ using tier4_autoware_utils::calcYawDeviation; AutonomousMode::AutonomousMode(rclcpp::Node * node) : logger_(node->get_logger()), clock_(node->get_clock()) { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); sub_control_cmd_ = node->create_subscription( "control_cmd", 1, [this](const Control::SharedPtr msg) { control_cmd_ = *msg; }); @@ -316,4 +316,4 @@ bool AutonomousMode::isModeChangeAvailable() return is_all_ok; } -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/autoware_operation_mode_transition_manager/src/state.hpp similarity index 88% rename from control/operation_mode_transition_manager/src/state.hpp rename to control/autoware_operation_mode_transition_manager/src/state.hpp index e5abd4285ad5f..a8f20ebf7369a 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/autoware_operation_mode_transition_manager/src/state.hpp @@ -15,11 +15,11 @@ #ifndef STATE_HPP_ #define STATE_HPP_ +#include "autoware_operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" #include "data.hpp" -#include "operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" +#include #include -#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace operation_mode_transition_manager +namespace autoware::operation_mode_transition_manager { class ModeChangeBase @@ -39,7 +39,8 @@ class ModeChangeBase virtual bool isModeChangeCompleted() = 0; virtual bool isModeChangeAvailable() = 0; - using DebugInfo = operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; + using DebugInfo = + autoware_operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; virtual DebugInfo getDebugInfo() { return DebugInfo{}; } }; @@ -83,7 +84,7 @@ class AutonomousMode : public ModeChangeBase Control trajectory_follower_control_cmd_; Odometry kinematics_; Trajectory trajectory_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; DebugInfo debug_info_; std::shared_ptr stable_start_time_; // Reset every transition start. @@ -105,6 +106,6 @@ class RemoteMode : public ModeChangeBase bool isModeChangeAvailable() override { return true; } }; -} // namespace operation_mode_transition_manager +} // namespace autoware::operation_mode_transition_manager #endif // STATE_HPP_ diff --git a/control/autoware_operation_mode_transition_manager/src/util.hpp b/control/autoware_operation_mode_transition_manager/src/util.hpp new file mode 100644 index 0000000000000..2e7e97a9e90d7 --- /dev/null +++ b/control/autoware_operation_mode_transition_manager/src/util.hpp @@ -0,0 +1,38 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTIL_HPP_ +#define UTIL_HPP_ + +#include "autoware_operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" + +using DebugInfo = + autoware_operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; + +void setAllOk(DebugInfo & debug_info) +{ + debug_info.is_all_ok = true; + debug_info.engage_allowed_for_stopped_vehicle = true; + debug_info.large_acceleration_ok = true; + debug_info.large_lateral_acceleration_diff_ok = true; + debug_info.large_lateral_acceleration_ok = true; + debug_info.lateral_deviation_ok = true; + debug_info.speed_lower_deviation_ok = true; + debug_info.speed_upper_deviation_ok = true; + debug_info.stop_ok = true; + debug_info.trajectory_available_ok = true; + debug_info.yaw_deviation_ok = true; +} + +#endif // UTIL_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/CMakeLists.txt b/control/autoware_pid_longitudinal_controller/CMakeLists.txt new file mode 100644 index 0000000000000..f0dce81eb54e1 --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_pid_longitudinal_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(PID_LON_CON_LIB ${PROJECT_NAME}_lib) +ament_auto_add_library(${PID_LON_CON_LIB} SHARED + DIRECTORY src +) + +if(BUILD_TESTING) + set(TEST_LON_SOURCES + test/test_pid.cpp + test/test_smooth_stop.cpp + test/test_longitudinal_controller_utils.cpp + ) + set(TEST_LONGITUDINAL_CONTROLLER_EXE test_longitudinal_controller) + ament_add_ros_isolated_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) + target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${PID_LON_CON_LIB}) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param +) diff --git a/control/autoware_pid_longitudinal_controller/README.md b/control/autoware_pid_longitudinal_controller/README.md new file mode 100644 index 0000000000000..9929d955e5ba7 --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/README.md @@ -0,0 +1,266 @@ +# PID Longitudinal Controller + +## Purpose / Use cases + +The longitudinal_controller computes the target acceleration to achieve the target velocity set at each point of the target trajectory using a feed-forward/back control. + +It also contains a slope force correction that takes into account road slope information, and a delay compensation function. +It is assumed that the target acceleration calculated here will be properly realized by the vehicle interface. + +Note that the use of this module is not mandatory for Autoware if the vehicle supports the "target speed" interface. + +## Design / Inner-workings / Algorithms + +### States + +This module has four state transitions as shown below in order to handle special processing in a specific situation. + +- **DRIVE** + - Executes target velocity tracking by PID control. + - It also applies the delay compensation and slope compensation. +- **STOPPING** + - Controls the motion just before stopping. + - Special sequence is performed to achieve accurate and smooth stopping. +- **STOPPED** + - Performs operations in the stopped state (e.g. brake hold) +- **EMERGENCY**. + - Enters an emergency state when certain conditions are met (e.g., when the vehicle has crossed a certain distance of a stop line). + - The recovery condition (whether or not to keep emergency state until the vehicle completely stops) or the deceleration in the emergency state are defined by parameters. + +The state transition diagram is shown below. + +![LongitudinalControllerStateTransition](./media/LongitudinalControllerStateTransition.drawio.svg) + +### Logics + +#### Control Block Diagram + +![LongitudinalControllerDiagram](./media/LongitudinalControllerDiagram.drawio.svg) + +#### FeedForward (FF) + +The reference acceleration set in the trajectory and slope compensation terms are output as a feedforward. Under ideal conditions with no modeling error, this FF term alone should be sufficient for velocity tracking. + +Tracking errors causing modeling or discretization errors are removed by the feedback control (now using PID). + +##### Brake keeping + +From the viewpoint of ride comfort, stopping with 0 acceleration is important because it reduces the impact of braking. However, if the target acceleration when stopping is 0, the vehicle may cross over the stop line or accelerate a little in front of the stop line due to vehicle model error or gradient estimation error. + +For reliable stopping, the target acceleration calculated by the FeedForward system is limited to a negative acceleration when stopping. + +![BrakeKeepingDiagram](./media/BrakeKeeping.drawio.svg) + +#### Slope compensation + +Based on the slope information, a compensation term is added to the target acceleration. + +There are two sources of the slope information, which can be switched by a parameter. + +- Pitch of the estimated ego-pose (default) + - Calculates the current slope from the pitch angle of the estimated ego-pose + - Pros: Easily available + - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. +- Z coordinate on the trajectory + - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory + - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained + - Pros: Can be used in combination with delay compensation (not yet implemented) + - Cons: z-coordinates of high-precision map is needed. + - Cons: Does not support free space planning (for now) + +**Notation:** This function works correctly only in a vehicle system that does not have acceleration feedback in the low-level control system. + +This compensation adds gravity correction to the target acceleration, resulting in an output value that is no longer equal to the target acceleration that the autonomous driving system desires. Therefore, it conflicts with the role of the acceleration feedback in the low-level controller. +For instance, if the vehicle is attempting to start with an acceleration of `1.0 m/s^2` and a gravity correction of `-1.0 m/s^2` is applied, the output value will be `0`. If this output value is mistakenly treated as the target acceleration, the vehicle will not start. + +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. + +This PID control calculates the target acceleration from the deviation between the current ego-velocity and the target velocity. + +This PID logic has a maximum value for the output of each term. This is to prevent the following: + +- Large integral terms may cause unintended behavior by users. +- Unintended noise may cause the output of the derivative term to be very large. + +Note: by default, the integral term in the control system is not accumulated when the vehicle is stationary. This precautionary measure aims to prevent unintended accumulation of the integral term in scenarios where Autoware assumes the vehicle is engaged, but an external system has immobilized the vehicle to initiate startup procedures. + +However, certain situations may arise, such as when the vehicle encounters a depression in the road surface during startup or if the slope compensation is inaccurately estimated (lower than necessary), leading to a failure to initiate motion. To address these scenarios, it is possible to activate error integration even when the vehicle is at rest by setting the `enable_integration_at_low_speed` parameter to true. + +When `enable_integration_at_low_speed` is set to true, the PID controller will initiate integration of the acceleration error after a specified duration defined by the `time_threshold_before_pid_integration` parameter has elapsed without the vehicle surpassing a minimum velocity set by the `current_vel_threshold_pid_integration` parameter. + +The presence of the `time_threshold_before_pid_integration` parameter is important for practical PID tuning. Integrating the error when the vehicle is stationary or at low speed can complicate PID tuning. This parameter effectively introduces a delay before the integral part becomes active, preventing it from kicking in immediately. This delay allows for more controlled and effective tuning of the PID controller. + +At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance. +This may be replaced by a higher performance controller (adaptive control or robust control) in future development. + +#### Time delay compensation + +At high speeds, the delay of actuator systems such as gas pedals and brakes has a significant impact on driving accuracy. +Depending on the actuating principle of the vehicle, the mechanism that physically controls the gas pedal and brake typically has a delay of about a hundred millisecond. + +In this controller, the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem. + +### Slope compensation + +Based on the slope information, a compensation term is added to the target acceleration. + +There are two sources of the slope information, which can be switched by a parameter. + +- Pitch of the estimated ego-pose (default) + - Calculates the current slope from the pitch angle of the estimated ego-pose + - Pros: Easily available + - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. +- Z coordinate on the trajectory + - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory + - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained + - Pros: Can be used in combination with delay compensation (not yet implemented) + - Cons: z-coordinates of high-precision map is needed. + - Cons: Does not support free space planning (for now) + +## Assumptions / Known limits + +1. Smoothed target velocity and its acceleration shall be set in the trajectory + 1. The velocity command is not smoothed inside the controller (only noise may be removed). + 2. For step-like target signal, tracking is performed as fast as possible. +2. The vehicle velocity must be an appropriate value + 1. The ego-velocity must be a signed-value corresponding to the forward/backward direction + 2. The ego-velocity should be given with appropriate noise processing. + 3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced. +3. The output of this controller must be achieved by later modules (e.g. vehicle interface). + 1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller. + +## Inputs / Outputs / API + +### Input + +Set the following from the [controller_node](../autoware_trajectory_follower_node/README.md) + +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry + +### Output + +Return LongitudinalOutput which contains the following to the controller node + +- `autoware_control_msgs/Longitudinal`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. +- LongitudinalSyncData + - velocity convergence(currently not used) + +### PIDController class + +The `PIDController` class is straightforward to use. +First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components. +Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function. + +## Parameter description + +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 | +| :------------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| delay_compensation_time | double | delay for longitudinal control [s] | 0.17 | +| enable_smooth_stop | bool | flag to enable transition to STOPPING | true | +| enable_overshoot_emergency | bool | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`. | true | +| enable_large_tracking_error_emergency | bool | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose. | true | +| enable_slope_compensation | bool | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true | +| enable_brake_keeping_before_stop | bool | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping). | false | +| enable_keep_stopped_until_steer_convergence | bool | flag to keep stopped condition until until the steer converges. | true | +| max_acc | double | max value of output acceleration [m/s^2] | 3.0 | +| min_acc | double | min value of output acceleration [m/s^2] | -5.0 | +| max_jerk | double | max value of jerk of output acceleration [m/s^3] | 2.0 | +| min_jerk | double | min value of jerk of output acceleration [m/s^3] | -5.0 | +| use_trajectory_for_pitch_calculation | bool | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used. | false | +| lpf_pitch_gain | double | gain of low-pass filter for pitch estimation | 0.95 | +| max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | +| min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | + +### State transition + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| drive_state_stop_dist | double | The state will transit to DRIVE when the distance to the stop point is longer than `drive_state_stop_dist` + `drive_state_offset_stop_dist` [m] | 0.5 | +| drive_state_offset_stop_dist | double | The state will transit to DRIVE when the distance to the stop point is longer than `drive_state_stop_dist` + `drive_state_offset_stop_dist` [m] | 1.0 | +| stopping_state_stop_dist | double | The state will transit to STOPPING when the distance to the stop point is shorter than `stopping_state_stop_dist` [m] | 0.5 | +| stopped_state_entry_vel | double | threshold of the ego velocity in transition to the STOPPED state [m/s] | 0.01 | +| stopped_state_entry_acc | double | threshold of the ego acceleration in transition to the STOPPED state [m/s^2] | 0.1 | +| emergency_state_overshoot_stop_dist | double | If `enable_overshoot_emergency` is true and the ego is `emergency_state_overshoot_stop_dist`-meter ahead of the stop point, the state will transit to EMERGENCY. [m] | 1.5 | +| emergency_state_traj_trans_dev | double | If the ego's position is `emergency_state_traj_tran_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m] | 3.0 | +| emergency_state_traj_rot_dev | double | If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad] | 0.784 | + +### DRIVE Parameter + +| Name | Type | Description | Default value | +| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| kp | double | p gain for longitudinal control | 1.0 | +| ki | double | i gain for longitudinal control | 0.1 | +| kd | double | d gain for longitudinal control | 0.0 | +| max_out | double | max value of PID's output acceleration during DRIVE state [m/s^2] | 1.0 | +| min_out | double | min value of PID's output acceleration during DRIVE state [m/s^2] | -1.0 | +| max_p_effort | double | max value of acceleration with p gain | 1.0 | +| min_p_effort | double | min value of acceleration with p gain | -1.0 | +| max_i_effort | double | max value of acceleration with i gain | 0.3 | +| min_i_effort | double | min value of acceleration with i gain | -0.3 | +| max_d_effort | double | max value of acceleration with d gain | 0.0 | +| min_d_effort | double | min value of acceleration with d gain | 0.0 | +| lpf_vel_error_gain | double | gain of low-pass filter for velocity error | 0.9 | +| enable_integration_at_low_speed | bool | Whether to enable integration of acceleration errors when the vehicle speed is lower than `current_vel_threshold_pid_integration` or not. | +| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | +| time_threshold_before_pid_integration | double | How much time without the vehicle moving must past to enable PID error integration. [s] | 5.0 | +| brake_keeping_acc | double | If `enable_brake_keeping_before_stop` is true, a certain acceleration is kept during DRIVE state before the ego stops [m/s^2] See [Brake keeping](#brake-keeping). | 0.2 | + +### STOPPING Parameter (smooth stop) + +Smooth stop is enabled if `enable_smooth_stop` is true. +In smooth stop, strong acceleration (`strong_acc`) will be output first to decrease the ego velocity. +Then weak acceleration (`weak_acc`) will be output to stop smoothly by decreasing the ego jerk. +If the ego does not stop in a certain time or some-meter over the stop point, weak acceleration to stop right (`weak_stop_acc`) now will be output. +If the ego is still running, strong acceleration (`strong_stop_acc`) to stop right now will be output. + +| Name | Type | Description | Default value | +| :--------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| smooth_stop_max_strong_acc | double | max strong acceleration [m/s^2] | -0.5 | +| smooth_stop_min_strong_acc | double | min strong acceleration [m/s^2] | -0.8 | +| smooth_stop_weak_acc | double | weak acceleration [m/s^2] | -0.3 | +| smooth_stop_weak_stop_acc | double | weak acceleration to stop right now [m/s^2] | -0.8 | +| smooth_stop_strong_stop_acc | double | strong acceleration to be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m/s^2] | -3.4 | +| smooth_stop_max_fast_vel | double | max fast vel to judge the ego is running fast [m/s]. If the ego is running fast, strong acceleration will be output. | 0.5 | +| smooth_stop_min_running_vel | double | min ego velocity to judge if the ego is running or not [m/s] | 0.01 | +| smooth_stop_min_running_acc | double | min ego acceleration to judge if the ego is running or not [m/s^2] | 0.01 | +| smooth_stop_weak_stop_time | double | max time to output weak acceleration [s]. After this, strong acceleration will be output. | 0.8 | +| smooth_stop_weak_stop_dist | double | Weak acceleration will be output when the ego is `smooth_stop_weak_stop_dist`-meter before the stop point. [m] | -0.3 | +| smooth_stop_strong_stop_dist | double | Strong acceleration will be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m] | -0.5 | + +### STOPPED Parameter + +The `STOPPED` state assumes that the vehicle is completely stopped with the brakes fully applied. +Therefore, `stopped_acc` should be set to a value that allows the vehicle to apply the strongest possible brake. +If `stopped_acc` is not sufficiently low, there is a possibility of sliding down on steep slopes. + +| Name | Type | Description | Default value | +| :----------- | :----- | :------------------------------------------- | :------------ | +| stopped_vel | double | target velocity in STOPPED state [m/s] | 0.0 | +| stopped_acc | double | target acceleration in STOPPED state [m/s^2] | -3.4 | +| stopped_jerk | double | target jerk in STOPPED state [m/s^3] | -5.0 | + +### EMERGENCY Parameter + +| Name | Type | Description | Default value | +| :------------- | :----- | :------------------------------------------------ | :------------ | +| emergency_vel | double | target velocity in EMERGENCY state [m/s] | 0.0 | +| emergency_acc | double | target acceleration in an EMERGENCY state [m/s^2] | -5.0 | +| emergency_jerk | double | target jerk in an EMERGENCY state [m/s^3] | -3.0 | + +## References / External links + +## Future extensions / Unimplemented parts + +## Related issues diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/debug_values.hpp similarity index 92% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/debug_values.hpp index 409af46b16ed1..bd682f9566134 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/debug_values.hpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ #include +#include namespace autoware::motion::control::pid_longitudinal_controller { @@ -92,4 +93,4 @@ class DebugValues }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp similarity index 96% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp index 4ea844a140f4f..171571b493774 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spherical_linear_interpolation.hpp" @@ -155,4 +155,4 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/lowpass_filter.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/lowpass_filter.hpp new file mode 100644 index 0000000000000..a743be573cf60 --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/lowpass_filter.hpp @@ -0,0 +1,66 @@ +// Copyright 2018-2021 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 AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ + +#include +#include +#include +#include + +namespace autoware::motion::control::pid_longitudinal_controller +{ + +/** + * @brief Simple filter with gain on the first derivative of the value + */ +class LowpassFilter1d +{ +private: + double m_x; //!< @brief current filtered value + double m_gain; //!< @brief gain value of first-order low-pass filter + +public: + /** + * @brief constructor with initial value and first-order gain + * @param [in] x initial value + * @param [in] gain first-order gain + */ + LowpassFilter1d(const double x, const double gain) : m_x(x), m_gain(gain) {} + + /** + * @brief set the current value of the filter + * @param [in] x new value + */ + void reset(const double x) { m_x = x; } + + /** + * @brief get the current value of the filter + */ + double getValue() const { return m_x; } + + /** + * @brief filter a new value + * @param [in] u new value + */ + double filter(const double u) + { + const double ret = m_gain * m_x + (1.0 - m_gain) * u; + m_x = ret; + return ret; + } +}; +} // namespace autoware::motion::control::pid_longitudinal_controller +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid.hpp new file mode 100644 index 0000000000000..8323e35b5da27 --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid.hpp @@ -0,0 +1,94 @@ +// Copyright 2018-2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_HPP_ + +#include + +namespace autoware::motion::control::pid_longitudinal_controller +{ + +/// @brief implementation of a PID controller +class PIDController +{ +public: + PIDController(); + + /** + * @brief calculate the output of this PID + * @param [in] error previous error + * @param [in] dt time step [s] + * @param [in] is_integrated if true, will use the integral component for calculation + * @param [out] pid_contributions values of the proportional, integral, and derivative components + * @return PID output + * @throw std::runtime_error if gains or limits have not been set + */ + double calculate( + const double error, const double dt, const bool is_integrated, + std::vector & pid_contributions); + /** + * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms + * @param [in] kp proportional coefficient + * @param [in] ki integral coefficient + * @param [in] kd derivative coefficient + */ + void setGains(const double kp, const double ki, const double kd); + /** + * @brief set limits on the total, proportional, integral, and derivative components + * @param [in] max_ret maximum return value of this PID + * @param [in] min_ret minimum return value of this PID + * @param [in] max_ret_p maximum value of the proportional component + * @param [in] min_ret_p minimum value of the proportional component + * @param [in] max_ret_i maximum value of the integral component + * @param [in] min_ret_i minimum value of the integral component + * @param [in] max_ret_d maximum value of the derivative component + * @param [in] min_ret_d minimum value of the derivative component + */ + void setLimits( + const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, + const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d); + /** + * @brief reset this PID to its initial state + */ + void reset(); + +private: + // PID parameters + struct Params + { + double kp; + double ki; + double kd; + double max_ret_p; + double min_ret_p; + double max_ret_i; + double min_ret_i; + double max_ret_d; + double min_ret_d; + double max_ret; + double min_ret; + }; + Params m_params; + + // state variables + double m_error_integral; + double m_prev_error; + bool m_is_first_time; + bool m_is_gains_set; + bool m_is_limits_set; +}; +} // namespace autoware::motion::control::pid_longitudinal_controller + +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp similarity index 94% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp index dee2e580e6911..98882509aaceb 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ - +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ + +#include "autoware_pid_longitudinal_controller/debug_values.hpp" +#include "autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp" +#include "autoware_pid_longitudinal_controller/lowpass_filter.hpp" +#include "autoware_pid_longitudinal_controller/pid.hpp" +#include "autoware_pid_longitudinal_controller/smooth_stop.hpp" +#include "autoware_trajectory_follower_base/longitudinal_controller_base.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" -#include "pid_longitudinal_controller/debug_values.hpp" -#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" -#include "pid_longitudinal_controller/lowpass_filter.hpp" -#include "pid_longitudinal_controller/pid.hpp" -#include "pid_longitudinal_controller/smooth_stop.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "trajectory_follower_base/longitudinal_controller_base.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -408,4 +408,4 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/smooth_stop.hpp similarity index 95% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/smooth_stop.hpp index 9a0a36123e143..e433fd158026b 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/smooth_stop.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ #include "rclcpp/rclcpp.hpp" @@ -112,4 +112,4 @@ class SmoothStop }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ diff --git a/control/pid_longitudinal_controller/media/BrakeKeeping.drawio.svg b/control/autoware_pid_longitudinal_controller/media/BrakeKeeping.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/BrakeKeeping.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/BrakeKeeping.drawio.svg diff --git a/control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg diff --git a/control/pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/autoware_pid_longitudinal_controller/media/slope_definition.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/slope_definition.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/slope_definition.drawio.svg diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml new file mode 100644 index 0000000000000..a84083eae2725 --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -0,0 +1,49 @@ + + + + autoware_pid_longitudinal_controller + 1.0.0 + PID-based longitudinal controller + + Takamasa Horibe + Takayuki Murooka + Mamoru Sobue + + Apache 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_trajectory_follower_base + autoware_vehicle_info_utils + autoware_vehicle_msgs + diagnostic_msgs + diagnostic_updater + eigen + geometry_msgs + interpolation + motion_utils + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml similarity index 100% rename from control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml rename to control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp similarity index 98% rename from control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp rename to control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 4e9ef52a4969e..0d8707afccad8 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" +#include "autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" diff --git a/control/autoware_pid_longitudinal_controller/src/pid.cpp b/control/autoware_pid_longitudinal_controller/src/pid.cpp new file mode 100644 index 0000000000000..6d2c61f639746 --- /dev/null +++ b/control/autoware_pid_longitudinal_controller/src/pid.cpp @@ -0,0 +1,105 @@ +// Copyright 2018-2021 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 "autoware_pid_longitudinal_controller/pid.hpp" + +#include +#include +#include +#include +#include + +namespace autoware::motion::control::pid_longitudinal_controller +{ +PIDController::PIDController() +: m_error_integral(0.0), + m_prev_error(0.0), + m_is_first_time(true), + m_is_gains_set(false), + m_is_limits_set(false) +{ +} + +double PIDController::calculate( + const double error, const double dt, const bool enable_integration, + std::vector & pid_contributions) +{ + if (!m_is_gains_set || !m_is_limits_set) { + throw std::runtime_error("Trying to calculate uninitialized PID"); + } + + const auto & p = m_params; + + double ret_p = p.kp * error; + ret_p = std::min(std::max(ret_p, p.min_ret_p), p.max_ret_p); + + if (enable_integration) { + m_error_integral += error * dt; + m_error_integral = std::min(std::max(m_error_integral, p.min_ret_i / p.ki), p.max_ret_i / p.ki); + } + const double ret_i = p.ki * m_error_integral; + + double error_differential; + if (m_is_first_time) { + error_differential = 0; + m_is_first_time = false; + } else { + error_differential = (error - m_prev_error) / dt; + } + double ret_d = p.kd * error_differential; + ret_d = std::min(std::max(ret_d, p.min_ret_d), p.max_ret_d); + + m_prev_error = error; + + pid_contributions.resize(3); + pid_contributions.at(0) = ret_p; + pid_contributions.at(1) = ret_i; + pid_contributions.at(2) = ret_d; + + double ret = ret_p + ret_i + ret_d; + ret = std::min(std::max(ret, p.min_ret), p.max_ret); + + return ret; +} + +void PIDController::setGains(const double kp, const double ki, const double kd) +{ + m_params.kp = kp; + m_params.ki = ki; + m_params.kd = kd; + m_is_gains_set = true; +} + +void PIDController::setLimits( + const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, + const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d) +{ + m_params.max_ret = max_ret; + m_params.min_ret = min_ret; + m_params.max_ret_p = max_ret_p; + m_params.min_ret_p = min_ret_p; + m_params.max_ret_d = max_ret_d; + m_params.min_ret_d = min_ret_d; + m_params.max_ret_i = max_ret_i; + m_params.min_ret_i = min_ret_i; + m_is_limits_set = true; +} + +void PIDController::reset() +{ + m_error_integral = 0.0; + m_prev_error = 0.0; + m_is_first_time = true; +} +} // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp similarity index 99% rename from control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp rename to control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 9a8223c610f9a..4cca525f6ebf3 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" +#include "autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -38,9 +38,11 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters timer m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double(); - m_wheel_base = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().wheel_base_m; - m_vehicle_width = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().vehicle_width_m; - m_front_overhang = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().front_overhang_m; + m_wheel_base = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().wheel_base_m; + m_vehicle_width = + autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().vehicle_width_m; + m_front_overhang = + autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().front_overhang_m; // parameters for delay compensation m_delay_compensation_time = node.declare_parameter("delay_compensation_time"); // [s] @@ -1148,7 +1150,7 @@ void PidLongitudinalController::updateDebugVelAcc(const ControlData & control_da void PidLongitudinalController::setupDiagnosticUpdater() { - diagnostic_updater_.setHardwareID("pid_longitudinal_controller"); + diagnostic_updater_.setHardwareID("autoware_pid_longitudinal_controller"); diagnostic_updater_.add("control_state", this, &PidLongitudinalController::checkControlState); } diff --git a/control/pid_longitudinal_controller/src/smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp similarity index 98% rename from control/pid_longitudinal_controller/src/smooth_stop.cpp rename to control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp index 27fab43fa506f..9d8ad05235f11 100644 --- a/control/pid_longitudinal_controller/src/smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pid_longitudinal_controller/smooth_stop.hpp" +#include "autoware_pid_longitudinal_controller/smooth_stop.hpp" #include // NOLINT diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp similarity index 99% rename from control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp rename to control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index bd79d7a8c3ac3..de4d32008aa28 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "gtest/gtest.h" #include "interpolation/spherical_linear_interpolation.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/control/pid_longitudinal_controller/test/test_pid.cpp b/control/autoware_pid_longitudinal_controller/test/test_pid.cpp similarity index 97% rename from control/pid_longitudinal_controller/test/test_pid.cpp rename to control/autoware_pid_longitudinal_controller/test/test_pid.cpp index 82d01e0028a9c..0e3e0c70945dc 100644 --- a/control/pid_longitudinal_controller/test/test_pid.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_pid.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_pid_longitudinal_controller/pid.hpp" #include "gtest/gtest.h" -#include "pid_longitudinal_controller/pid.hpp" #include diff --git a/control/pid_longitudinal_controller/test/test_smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp similarity index 98% rename from control/pid_longitudinal_controller/test/test_smooth_stop.cpp rename to control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp index d658a5271d249..4f4fbb2354fa1 100644 --- a/control/pid_longitudinal_controller/test/test_smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_pid_longitudinal_controller/smooth_stop.hpp" #include "gtest/gtest.h" -#include "pid_longitudinal_controller/smooth_stop.hpp" #include "rclcpp/rclcpp.hpp" #include diff --git a/control/autoware_pure_pursuit/CMakeLists.txt b/control/autoware_pure_pursuit/CMakeLists.txt new file mode 100644 index 0000000000000..eac7c723901aa --- /dev/null +++ b/control/autoware_pure_pursuit/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_pure_pursuit) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIRS} +) + +# autoware_pure_pursuit_core +ament_auto_add_library(${PROJECT_NAME}_core SHARED + src/${PROJECT_NAME}_core/planning_utils.cpp + src/${PROJECT_NAME}_core/${PROJECT_NAME}.cpp + src/${PROJECT_NAME}_core/interpolate.cpp +) + +# autoware_pure_pursuit +ament_auto_add_library(${PROJECT_NAME}_lateral_controller SHARED + src/${PROJECT_NAME}/${PROJECT_NAME}_lateral_controller.cpp + src/${PROJECT_NAME}/${PROJECT_NAME}_viz.cpp +) + +target_link_libraries(${PROJECT_NAME}_lateral_controller + ${PROJECT_NAME}_core +) + +rclcpp_components_register_node(${PROJECT_NAME}_lateral_controller + PLUGIN "autoware::pure_pursuit::PurePursuitLateralController" + EXECUTABLE ${PROJECT_NAME}_lateral_controller_exe +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_pure_pursuit/README.md b/control/autoware_pure_pursuit/README.md new file mode 100644 index 0000000000000..3741abbf2a766 --- /dev/null +++ b/control/autoware_pure_pursuit/README.md @@ -0,0 +1,19 @@ +# Pure Pursuit Controller + +The Pure Pursuit Controller module calculates the steering angle for tracking a desired trajectory using the pure pursuit algorithm. This is used as a lateral controller plugin in the `autoware_trajectory_follower_node`. + +## Inputs + +Set the following from the [controller_node](../autoware_trajectory_follower_node/README.md) + +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current ego pose and velocity information + +## Outputs + +Return LateralOutput which contains the following to the controller node + +- `autoware_control_msgs/Lateral`: target steering angle +- LateralSyncData + - steer angle convergence +- `autoware_planning_msgs/Trajectory`: predicted path for ego vehicle diff --git a/control/pure_pursuit/config/pure_pursuit.param.yaml b/control/autoware_pure_pursuit/config/pure_pursuit.param.yaml similarity index 100% rename from control/pure_pursuit/config/pure_pursuit.param.yaml rename to control/autoware_pure_pursuit/config/pure_pursuit.param.yaml diff --git a/control/pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml b/control/autoware_pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml similarity index 100% rename from control/pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml rename to control/autoware_pure_pursuit/config/pure_pursuit_lateral_controller_plotjuggler_debug.xml diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit.hpp b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit.hpp new file mode 100644 index 0000000000000..7998cbefa4e39 --- /dev/null +++ b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit.hpp @@ -0,0 +1,88 @@ +// 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. +/* + * Copyright 2015-2019 Autoware Foundation. 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 AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ +#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ + +#include + +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +namespace autoware::pure_pursuit +{ +class PurePursuit +{ +public: + PurePursuit() : lookahead_distance_(0.0), closest_thr_dist_(3.0), closest_thr_ang_(M_PI / 4) {} + ~PurePursuit() = default; + + rclcpp::Logger logger = rclcpp::get_logger("pure_pursuit"); + // setter + void setCurrentPose(const geometry_msgs::msg::Pose & msg); + void setWaypoints(const std::vector & msg); + void setLookaheadDistance(double ld) { lookahead_distance_ = ld; } + void setClosestThreshold(double closest_thr_dist, double closest_thr_ang) + { + closest_thr_dist_ = closest_thr_dist; + closest_thr_ang_ = closest_thr_ang; + } + + // getter + geometry_msgs::msg::Point getLocationOfNextWaypoint() const { return loc_next_wp_; } + geometry_msgs::msg::Point getLocationOfNextTarget() const { return loc_next_tgt_; } + + bool isDataReady(); + std::pair run(); // calculate curvature + +private: + // variables for debug + geometry_msgs::msg::Point loc_next_wp_; + geometry_msgs::msg::Point loc_next_tgt_; + + // variables got from outside + double lookahead_distance_, closest_thr_dist_, closest_thr_ang_; + std::shared_ptr> curr_wps_ptr_; + std::shared_ptr curr_pose_ptr_; + + // functions + int32_t findNextPointIdx(int32_t search_start_idx); + std::pair lerpNextTarget(int32_t next_wp_idx); +}; + +} // namespace autoware::pure_pursuit + +#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp new file mode 100644 index 0000000000000..f5c6ff50712eb --- /dev/null +++ b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp @@ -0,0 +1,179 @@ +// Copyright 2020-2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// 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 2015-2019 Autoware Foundation. 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 AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ +#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ + +#include "autoware_pure_pursuit/autoware_pure_pursuit.hpp" +#include "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp" +#include "autoware_trajectory_follower_base/lateral_controller_base.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include +#include +#include + +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include // To be replaced by std::optional in C++17 + +#include +#include + +using autoware::motion::control::trajectory_follower::InputData; +using autoware::motion::control::trajectory_follower::LateralControllerBase; +using autoware::motion::control::trajectory_follower::LateralOutput; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; + +namespace autoware::pure_pursuit +{ + +struct PpOutput +{ + double curvature; + double velocity; +}; + +struct Param +{ + // Global Parameters + double wheel_base; + double max_steering_angle; // [rad] + + // Algorithm Parameters + double ld_velocity_ratio; + double ld_lateral_error_ratio; + double ld_curvature_ratio; + double min_lookahead_distance; + double max_lookahead_distance; + double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear + double converged_steer_rad_; + double prediction_ds; + double prediction_distance_length; // Total distance of prediction trajectory + double resampling_ds; + double curvature_calculation_distance; + double long_ld_lateral_error_threshold; + bool enable_path_smoothing; + int path_filter_moving_ave_num; +}; + +struct DebugData +{ + geometry_msgs::msg::Point next_target; +}; + +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::Clock::SharedPtr clock_; + rclcpp::Logger logger_; + std::vector output_tp_array_; + autoware_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; + autoware_planning_msgs::msg::Trajectory trajectory_; + nav_msgs::msg::Odometry current_odometry_; + autoware_vehicle_msgs::msg::SteeringReport current_steering_; + boost::optional prev_cmd_; + + // Debug Publisher + rclcpp::Publisher::SharedPtr pub_debug_marker_; + rclcpp::Publisher::SharedPtr pub_debug_values_; + // Predicted Trajectory publish + rclcpp::Publisher::SharedPtr pub_predicted_trajectory_; + + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + + void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + void setResampledTrajectory(); + + // TF + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + geometry_msgs::msg::Pose current_pose_; + + void publishDebugMarker() const; + + /** + * @brief compute control command for path follow with a constant control period + */ + bool isReady([[maybe_unused]] const InputData & input_data) override; + LateralOutput run(const InputData & input_data) override; + + Lateral generateCtrlCmdMsg(const double target_curvature); + + // Parameter + Param param_{}; + + // Algorithm + std::unique_ptr pure_pursuit_; + + boost::optional calcTargetCurvature( + bool is_control_output, geometry_msgs::msg::Pose pose); + + /** + * @brief It takes current pose, control command, and delta distance. Then it calculates next pose + * of vehicle. + */ + + TrajectoryPoint calcNextPose(const double ds, TrajectoryPoint & point, Lateral cmd) const; + + boost::optional generatePredictedTrajectory(); + + Lateral generateOutputControlCmd(); + + bool calcIsSteerConverged(const Lateral & cmd); + + double calcLookaheadDistance( + const double lateral_error, const double curvature, const double velocity, const double min_ld, + const bool is_control_cmd); + + double calcCurvature(const size_t closest_idx); + + void averageFilterTrajectory(autoware_planning_msgs::msg::Trajectory & u); + + // Debug + mutable DebugData debug_data_; +}; + +} // namespace autoware::pure_pursuit + +#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_node.hpp b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_node.hpp new file mode 100644 index 0000000000000..1a67dbe1a178b --- /dev/null +++ b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_node.hpp @@ -0,0 +1,129 @@ +// 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. +/* + * Copyright 2015-2019 Autoware Foundation. 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 AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_ +#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_ + +#include "autoware_pure_pursuit/autoware_pure_pursuit.hpp" +#include "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include // To be replaced by std::optional in C++17 + +#include +#include + +#include + +namespace autoware::pure_pursuit +{ +struct Param +{ + // Global Parameters + double wheel_base; + + // Node Parameters + double ctrl_period; + + // Algorithm Parameters + double lookahead_distance_ratio; + double min_lookahead_distance; + double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear +}; + +struct DebugData +{ + geometry_msgs::msg::Point next_target; +}; + +class PurePursuitNode : public rclcpp::Node +{ +public: + explicit PurePursuitNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_trajectory_{this, "input/reference_trajectory"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_current_odometry_{this, "input/current_odometry"}; + + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; + nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; + + bool isDataReady(); + + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + // TF + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; + + // Publisher + rclcpp::Publisher::SharedPtr pub_ctrl_cmd_; + + void publishCommand(const double target_curvature); + + // Debug Publisher + rclcpp::Publisher::SharedPtr pub_debug_marker_; + + void publishDebugMarker() const; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void onTimer(); + + // Parameter + Param param_; + + // Algorithm + std::unique_ptr pure_pursuit_; + + boost::optional calcTargetCurvature(); + boost::optional calcTargetPoint() const; + + // Debug + mutable DebugData debug_data_; +}; + +} // namespace autoware::pure_pursuit + +#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp new file mode 100644 index 0000000000000..e5fe80d9dc68c --- /dev/null +++ b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp @@ -0,0 +1,47 @@ +// 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. +/* + * Copyright 2015-2019 Autoware Foundation. 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 AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ +#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ + +#include +#include +#include + +#include +namespace autoware::pure_pursuit +{ +visualization_msgs::msg::Marker createNextTargetMarker( + const geometry_msgs::msg::Point & next_target); + +visualization_msgs::msg::Marker createTrajectoryCircleMarker( + const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose); +} // namespace autoware::pure_pursuit + +#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/util/interpolate.hpp b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/interpolate.hpp similarity index 88% rename from control/pure_pursuit/include/pure_pursuit/util/interpolate.hpp rename to control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/interpolate.hpp index b62a0336e5de7..eb7703b56c1a2 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/interpolate.hpp +++ b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/interpolate.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ -#define PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ +#ifndef AUTOWARE_PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ +#define AUTOWARE_PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ #include #include #include -namespace pure_pursuit +namespace autoware::pure_pursuit { class LinearInterpolate { @@ -50,6 +50,6 @@ class SplineInterpolate const std::vector & return_index, std::vector & return_value); void getValueVector(const std::vector & s_v, std::vector & value_v); }; -} // namespace pure_pursuit +} // namespace autoware::pure_pursuit -#endif // PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ +#endif // AUTOWARE_PURE_PURSUIT__UTIL__INTERPOLATE_HPP_ diff --git a/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/marker_helper.hpp b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/marker_helper.hpp new file mode 100644 index 0000000000000..d265ab751313a --- /dev/null +++ b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/marker_helper.hpp @@ -0,0 +1,107 @@ +// 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 AUTOWARE_PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ +#define AUTOWARE_PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ + +#include + +#include + +#include + +namespace autoware::pure_pursuit +{ +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 createDefaultMarker( + const std::string & frame_id, 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; + // ToDo + // marker.header.stamp = rclcpp::Node::now(); + marker.ns = ns; + marker.id = id; + marker.type = type; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0); + + 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; +} + +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 autoware::pure_pursuit + +#endif // AUTOWARE_PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/planning_utils.hpp similarity index 94% rename from control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp rename to control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/planning_utils.hpp index b2d6e13c585f4..e5f7072435c9c 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp +++ b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/planning_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ -#define PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ +#ifndef AUTOWARE_PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ +#define AUTOWARE_PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ #define EIGEN_MPL2_ONLY @@ -45,7 +45,7 @@ #define PLANNING_UTILS_LOGGER "planning_utils" -namespace pure_pursuit +namespace autoware::pure_pursuit { namespace planning_utils { @@ -139,6 +139,6 @@ geometry_msgs::msg::Point transformToRelativeCoordinate2D( geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double _yaw); } // namespace planning_utils -} // namespace pure_pursuit +} // namespace autoware::pure_pursuit -#endif // PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ +#endif // AUTOWARE_PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/util/tf_utils.hpp b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/tf_utils.hpp similarity index 92% rename from control/pure_pursuit/include/pure_pursuit/util/tf_utils.hpp rename to control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/tf_utils.hpp index b63bff5b6a6c6..b5b635feebcc7 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/tf_utils.hpp +++ b/control/autoware_pure_pursuit/include/autoware_pure_pursuit/util/tf_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PURE_PURSUIT__UTIL__TF_UTILS_HPP_ -#define PURE_PURSUIT__UTIL__TF_UTILS_HPP_ +#ifndef AUTOWARE_PURE_PURSUIT__UTIL__TF_UTILS_HPP_ +#define AUTOWARE_PURE_PURSUIT__UTIL__TF_UTILS_HPP_ #include @@ -31,7 +31,7 @@ #define TF_UTILS_LOGGER "tf_utils" -namespace pure_pursuit +namespace autoware::pure_pursuit { namespace tf_utils { @@ -92,6 +92,6 @@ inline boost::optional getCurrentPose( } } // namespace tf_utils -} // namespace pure_pursuit +} // namespace autoware::pure_pursuit -#endif // PURE_PURSUIT__UTIL__TF_UTILS_HPP_ +#endif // AUTOWARE_PURE_PURSUIT__UTIL__TF_UTILS_HPP_ diff --git a/control/pure_pursuit/launch/pure_pursuit.launch.xml b/control/autoware_pure_pursuit/launch/pure_pursuit.launch.xml similarity index 100% rename from control/pure_pursuit/launch/pure_pursuit.launch.xml rename to control/autoware_pure_pursuit/launch/pure_pursuit.launch.xml diff --git a/control/autoware_pure_pursuit/package.xml b/control/autoware_pure_pursuit/package.xml new file mode 100644 index 0000000000000..138eda06926b5 --- /dev/null +++ b/control/autoware_pure_pursuit/package.xml @@ -0,0 +1,44 @@ + + + + autoware_pure_pursuit + 0.1.0 + The pure_pursuit package + Takamasa Horibe + Takayuki Murooka + Apache License 2.0 + + Berkay Karaman + Takamasa Horibe + + ament_cmake_auto + autoware_cmake + + autoware_control_msgs + autoware_planning_msgs + autoware_trajectory_follower_base + autoware_vehicle_info_utils + boost + geometry_msgs + libboost-dev + motion_utils + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp new file mode 100644 index 0000000000000..ba1d97bf66d28 --- /dev/null +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp @@ -0,0 +1,490 @@ +// Copyright 2020-2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// +// 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 2015-2019 Autoware Foundation. 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 "autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp" + +#include "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp" +#include "autoware_pure_pursuit/util/planning_utils.hpp" +#include "autoware_pure_pursuit/util/tf_utils.hpp" + +#include + +#include +#include +#include + +namespace +{ +enum TYPE { + VEL_LD = 0, + CURVATURE_LD = 1, + LATERAL_ERROR_LD = 2, + TOTAL_LD = 3, + CURVATURE = 4, + LATERAL_ERROR = 5, + VELOCITY = 6, + SIZE // this is the number of enum elements +}; +} // namespace + +namespace autoware::pure_pursuit +{ +PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) +: 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 = autoware::vehicle_info_utils::VehicleInfoUtils(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_.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"); + 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"); + 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"); + + // Debug Publishers + pub_debug_marker_ = + 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( + "~/output/predicted_trajectory", 1); +} + +double PurePursuitLateralController::calcLookaheadDistance( + const double lateral_error, const double curvature, const double velocity, const double min_ld, + const bool is_control_cmd) +{ + const double vel_ld = abs(param_.ld_velocity_ratio * velocity); + const double curvature_ld = -abs(param_.ld_curvature_ratio * curvature); + double lateral_error_ld = 0.0; + + if (abs(lateral_error) >= param_.long_ld_lateral_error_threshold) { + // If lateral error is higher than threshold, we should make ld larger to prevent entering the + // road with high heading error. + lateral_error_ld = abs(param_.ld_lateral_error_ratio * lateral_error); + } + + const double total_ld = + std::clamp(vel_ld + curvature_ld + lateral_error_ld, min_ld, param_.max_lookahead_distance); + + auto pubDebugValues = [&]() { + tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + debug_msg.data.resize(TYPE::SIZE); + debug_msg.data.at(TYPE::VEL_LD) = static_cast(vel_ld); + debug_msg.data.at(TYPE::CURVATURE_LD) = static_cast(curvature_ld); + debug_msg.data.at(TYPE::LATERAL_ERROR_LD) = static_cast(lateral_error_ld); + debug_msg.data.at(TYPE::TOTAL_LD) = static_cast(total_ld); + 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 = clock_->now(); + pub_debug_values_->publish(debug_msg); + }; + + if (is_control_cmd) { + pubDebugValues(); + } + + return total_ld; +} + +TrajectoryPoint PurePursuitLateralController::calcNextPose( + const double ds, TrajectoryPoint & point, Lateral cmd) const +{ + geometry_msgs::msg::Transform transform; + transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); + transform.rotation = + planning_utils::getQuaternionFromYaw(((tan(cmd.steering_tire_angle) * ds) / param_.wheel_base)); + TrajectoryPoint output_p; + + tf2::Transform tf_pose; + tf2::Transform tf_offset; + tf2::fromMsg(transform, tf_offset); + tf2::fromMsg(point.pose, tf_pose); + tf2::toMsg(tf_pose * tf_offset, output_p.pose); + return output_p; +} + +void PurePursuitLateralController::setResampledTrajectory() +{ + // Interpolate with constant interval distance. + std::vector out_arclength; + const auto input_tp_array = motion_utils::convertToTrajectoryPointArray(trajectory_); + const auto traj_length = motion_utils::calcArcLength(input_tp_array); + for (double s = 0; s < traj_length; s += param_.resampling_ds) { + out_arclength.push_back(s); + } + trajectory_resampled_ = + std::make_shared(motion_utils::resampleTrajectory( + motion_utils::convertToTrajectory(input_tp_array), out_arclength)); + trajectory_resampled_->points.back() = trajectory_.points.back(); + trajectory_resampled_->header = trajectory_.header; + output_tp_array_ = motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); +} + +double PurePursuitLateralController::calcCurvature(const size_t closest_idx) +{ + // Calculate current curvature + const size_t idx_dist = static_cast( + std::max(static_cast((param_.curvature_calculation_distance) / param_.resampling_ds), 1)); + + // Find the points in trajectory to calculate curvature + size_t next_idx = trajectory_resampled_->points.size() - 1; + size_t prev_idx = 0; + + if (static_cast(closest_idx) >= idx_dist) { + prev_idx = closest_idx - idx_dist; + } else { + // return zero curvature when backward distance is not long enough in the trajectory + return 0.0; + } + + if (trajectory_resampled_->points.size() - 1 >= closest_idx + idx_dist) { + next_idx = closest_idx + idx_dist; + } else { + // return zero curvature when forward distance is not long enough in the trajectory + return 0.0; + } + // TODO(k.sugahara): shift the center point of the curvature calculation to allow sufficient + // distance, because if sufficient distance cannot be obtained in front or behind, the curvature + // will be zero in the current implementation. + + // Calculate curvature assuming the trajectory points interval is constant + double current_curvature = 0.0; + + try { + current_curvature = tier4_autoware_utils::calcCurvature( + tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(prev_idx)), + tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(closest_idx)), + tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(next_idx))); + } catch (std::exception const & e) { + // ...code that handles the error... + RCLCPP_WARN(rclcpp::get_logger("pure_pursuit"), "%s", e.what()); + current_curvature = 0.0; + } + return current_curvature; +} + +void PurePursuitLateralController::averageFilterTrajectory( + autoware_planning_msgs::msg::Trajectory & u) +{ + if (static_cast(u.points.size()) <= 2 * param_.path_filter_moving_ave_num) { + RCLCPP_ERROR(logger_, "Cannot smooth path! Trajectory size is too low!"); + return; + } + + autoware_planning_msgs::msg::Trajectory filtered_trajectory(u); + + for (int64_t i = 0; i < static_cast(u.points.size()); ++i) { + TrajectoryPoint tmp{}; + int64_t num_tmp = param_.path_filter_moving_ave_num; + int64_t count = 0; + double yaw = 0.0; + if (i - num_tmp < 0) { + num_tmp = i; + } + if (i + num_tmp > static_cast(u.points.size()) - 1) { + num_tmp = static_cast(u.points.size()) - i - 1; + } + for (int64_t j = -num_tmp; j <= num_tmp; ++j) { + const auto & p = u.points.at(static_cast(i + j)); + + tmp.pose.position.x += p.pose.position.x; + tmp.pose.position.y += p.pose.position.y; + tmp.pose.position.z += p.pose.position.z; + tmp.longitudinal_velocity_mps += p.longitudinal_velocity_mps; + tmp.acceleration_mps2 += p.acceleration_mps2; + tmp.front_wheel_angle_rad += p.front_wheel_angle_rad; + tmp.heading_rate_rps += p.heading_rate_rps; + yaw += tf2::getYaw(p.pose.orientation); + tmp.lateral_velocity_mps += p.lateral_velocity_mps; + tmp.rear_wheel_angle_rad += p.rear_wheel_angle_rad; + ++count; + } + auto & p = filtered_trajectory.points.at(static_cast(i)); + + p.pose.position.x = tmp.pose.position.x / count; + p.pose.position.y = tmp.pose.position.y / count; + p.pose.position.z = tmp.pose.position.z / count; + p.longitudinal_velocity_mps = tmp.longitudinal_velocity_mps / count; + p.acceleration_mps2 = tmp.acceleration_mps2 / count; + p.front_wheel_angle_rad = tmp.front_wheel_angle_rad / count; + p.heading_rate_rps = tmp.heading_rate_rps / count; + p.lateral_velocity_mps = tmp.lateral_velocity_mps / count; + p.rear_wheel_angle_rad = tmp.rear_wheel_angle_rad / count; + p.pose.orientation = autoware::pure_pursuit::planning_utils::getQuaternionFromYaw(yaw / count); + } + trajectory_resampled_ = std::make_shared(filtered_trajectory); +} + +boost::optional PurePursuitLateralController::generatePredictedTrajectory() +{ + const auto closest_idx_result = + motion_utils::findNearestIndex(output_tp_array_, current_odometry_.pose.pose, 3.0, M_PI_4); + + if (!closest_idx_result) { + return boost::none; + } + + const double remaining_distance = planning_utils::calcArcLengthFromWayPoint( + *trajectory_resampled_, *closest_idx_result, trajectory_resampled_->points.size() - 1); + + const auto num_of_iteration = std::max( + static_cast(std::ceil( + std::min(remaining_distance, param_.prediction_distance_length) / param_.prediction_ds)), + 1); + Trajectory predicted_trajectory; + + // Iterative prediction: + for (int i = 0; i < num_of_iteration; i++) { + if (i == 0) { + // For first point, use the odometry for velocity, and use the current_pose for prediction. + + TrajectoryPoint p; + p.pose = current_odometry_.pose.pose; + p.longitudinal_velocity_mps = current_odometry_.twist.twist.linear.x; + predicted_trajectory.points.push_back(p); + + const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose); + Lateral tmp_msg; + + if (pp_output) { + tmp_msg = generateCtrlCmdMsg(pp_output->curvature); + predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; + } else { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); + tmp_msg = generateCtrlCmdMsg(0.0); + } + TrajectoryPoint p2; + p2 = calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg); + predicted_trajectory.points.push_back(p2); + + } else { + const auto pp_output = calcTargetCurvature(false, predicted_trajectory.points.at(i).pose); + Lateral tmp_msg; + + if (pp_output) { + tmp_msg = generateCtrlCmdMsg(pp_output->curvature); + predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; + } else { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); + tmp_msg = generateCtrlCmdMsg(0.0); + } + predicted_trajectory.points.push_back( + calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg)); + } + } + + // for last point + predicted_trajectory.points.back().longitudinal_velocity_mps = 0.0; + predicted_trajectory.header.frame_id = trajectory_resampled_->header.frame_id; + predicted_trajectory.header.stamp = trajectory_resampled_->header.stamp; + + return predicted_trajectory; +} + +bool PurePursuitLateralController::isReady([[maybe_unused]] const InputData & input_data) +{ + return true; +} + +LateralOutput PurePursuitLateralController::run(const InputData & input_data) +{ + current_pose_ = input_data.current_odometry.pose.pose; + trajectory_ = input_data.current_trajectory; + current_odometry_ = input_data.current_odometry; + current_steering_ = input_data.current_steering; + + setResampledTrajectory(); + if (param_.enable_path_smoothing) { + averageFilterTrajectory(*trajectory_resampled_); + } + const auto cmd_msg = generateOutputControlCmd(); + + LateralOutput output; + output.control_cmd = cmd_msg; + output.sync_data.is_steer_converged = calcIsSteerConverged(cmd_msg); + + // calculate predicted trajectory with iterative calculation + const auto predicted_trajectory = generatePredictedTrajectory(); + if (!predicted_trajectory) { + RCLCPP_ERROR(logger_, "Failed to generate predicted trajectory."); + } else { + pub_predicted_trajectory_->publish(*predicted_trajectory); + } + + return output; +} + +bool PurePursuitLateralController::calcIsSteerConverged(const Lateral & cmd) +{ + return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) < + static_cast(param_.converged_steer_rad_); +} + +Lateral PurePursuitLateralController::generateOutputControlCmd() +{ + // Generate the control command + const auto pp_output = calcTargetCurvature(true, current_odometry_.pose.pose); + Lateral output_cmd; + + if (pp_output) { + output_cmd = generateCtrlCmdMsg(pp_output->curvature); + prev_cmd_ = boost::optional(output_cmd); + publishDebugMarker(); + } else { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, "failed to solve pure_pursuit for control command calculation"); + if (prev_cmd_) { + output_cmd = *prev_cmd_; + } else { + output_cmd = generateCtrlCmdMsg(0.0); + } + } + return output_cmd; +} + +Lateral PurePursuitLateralController::generateCtrlCmdMsg(const double target_curvature) +{ + const double tmp_steering = + planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); + Lateral cmd; + cmd.stamp = clock_->now(); + cmd.steering_tire_angle = static_cast( + std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle)); + + // pub_ctrl_cmd_->publish(cmd); + return cmd; +} + +void PurePursuitLateralController::publishDebugMarker() const +{ + visualization_msgs::msg::MarkerArray marker_array; + + marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); + marker_array.markers.push_back( + createTrajectoryCircleMarker(debug_data_.next_target, current_odometry_.pose.pose)); +} + +boost::optional PurePursuitLateralController::calcTargetCurvature( + bool is_control_output, geometry_msgs::msg::Pose pose) +{ + // Ignore invalid trajectory + if (trajectory_resampled_->points.size() < 3) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "received path size is < 3, ignored"); + return {}; + } + + // Calculate target point for velocity/acceleration + + const auto closest_idx_result = + motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4); + if (!closest_idx_result) { + RCLCPP_ERROR(logger_, "cannot find closest waypoint"); + return {}; + } + + const double target_vel = + trajectory_resampled_->points.at(*closest_idx_result).longitudinal_velocity_mps; + + // calculate the lateral error + + const double lateral_error = + motion_utils::calcLateralOffset(trajectory_resampled_->points, pose.position); + + // calculate the current curvature + + const double current_curvature = calcCurvature(*closest_idx_result); + + // Calculate lookahead distance + + const bool is_reverse = (target_vel < 0); + const double min_lookahead_distance = + is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; + double lookahead_distance = min_lookahead_distance; + if (is_control_output) { + lookahead_distance = calcLookaheadDistance( + lateral_error, current_curvature, current_odometry_.twist.twist.linear.x, + min_lookahead_distance, is_control_output); + } else { + lookahead_distance = calcLookaheadDistance( + lateral_error, current_curvature, target_vel, min_lookahead_distance, is_control_output); + } + + // Set PurePursuit data + pure_pursuit_->setCurrentPose(pose); + pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_resampled_)); + pure_pursuit_->setLookaheadDistance(lookahead_distance); + + // Run PurePursuit + const auto pure_pursuit_result = pure_pursuit_->run(); + if (!pure_pursuit_result.first) { + return {}; + } + + const auto kappa = pure_pursuit_result.second; + + // Set debug data + if (is_control_output) { + debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); + } + PpOutput output{}; + output.curvature = kappa; + if (!is_control_output) { + output.velocity = current_odometry_.twist.twist.linear.x; + } else { + output.velocity = target_vel; + } + + return output; +} +} // namespace autoware::pure_pursuit diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp new file mode 100644 index 0000000000000..23ce643e18eed --- /dev/null +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_node.cpp @@ -0,0 +1,214 @@ +// 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. + +/* + * Copyright 2015-2019 Autoware Foundation. 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 "autoware_pure_pursuit/autoware_pure_pursuit_node.hpp" + +#include "autoware_pure_pursuit/pure_pursuit_viz.hpp" +#include "autoware_pure_pursuit/util/planning_utils.hpp" +#include "autoware_pure_pursuit/util/tf_utils.hpp" + +#include + +#include +#include +#include + +namespace +{ +double calcLookaheadDistance( + const double velocity, const double lookahead_distance_ratio, const double min_lookahead_distance) +{ + const double lookahead_distance = lookahead_distance_ratio * std::abs(velocity); + return std::max(lookahead_distance, min_lookahead_distance); +} + +} // namespace + +namespace autoware::pure_pursuit +{ +PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) +: Node("pure_pursuit", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) +{ + pure_pursuit_ = std::make_unique(); + + // Vehicle Parameters + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); + param_.wheel_base = vehicle_info.wheel_base_m; + + // Node Parameters + param_.ctrl_period = this->declare_parameter("control_period"); + + // Algorithm Parameters + param_.lookahead_distance_ratio = this->declare_parameter("lookahead_distance_ratio"); + param_.min_lookahead_distance = this->declare_parameter("min_lookahead_distance"); + param_.reverse_min_lookahead_distance = + this->declare_parameter("reverse_min_lookahead_distance"); + + // Publishers + pub_ctrl_cmd_ = + this->create_publisher("output/control_raw", 1); + + // Debug Publishers + pub_debug_marker_ = + this->create_publisher("~/debug/markers", 0); + + // Timer + { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(param_.ctrl_period)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&PurePursuitNode::onTimer, this)); + } + + // Wait for first current pose + tf_utils::waitForTransform(tf_buffer_, "map", "base_link"); +} + +bool PurePursuitNode::isDataReady() +{ + if (!current_odometry_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_odometry..."); + return false; + } + + if (!trajectory_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for trajectory..."); + return false; + } + + if (!current_pose_) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_pose..."); + return false; + } + + return true; +} + +void PurePursuitNode::onTimer() +{ + current_pose_ = self_pose_listener_.getCurrentPose(); + + current_odometry_ = sub_current_odometry_.takeData(); + trajectory_ = sub_trajectory_.takeData(); + if (!isDataReady()) { + return; + } + + const auto target_curvature = calcTargetCurvature(); + + if (target_curvature) { + publishCommand(*target_curvature); + publishDebugMarker(); + } else { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "failed to solve pure_pursuit"); + publishCommand({0.0}); + } +} + +void PurePursuitNode::publishCommand(const double target_curvature) +{ + autoware_control_msgs::msg::Lateral cmd; + cmd.stamp = get_clock()->now(); + cmd.steering_tire_angle = + planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); + pub_ctrl_cmd_->publish(cmd); +} + +void PurePursuitNode::publishDebugMarker() const +{ + visualization_msgs::msg::MarkerArray marker_array; + + marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); + marker_array.markers.push_back( + createTrajectoryCircleMarker(debug_data_.next_target, current_pose_->pose)); + + pub_debug_marker_->publish(marker_array); +} + +boost::optional PurePursuitNode::calcTargetCurvature() +{ + // Ignore invalid trajectory + if (trajectory_->points.size() < 3) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "received path size is < 3, ignored"); + return {}; + } + + // Calculate target point for velocity/acceleration + const auto target_point = calcTargetPoint(); + if (!target_point) { + return {}; + } + + const double target_vel = target_point->longitudinal_velocity_mps; + + // Calculate lookahead distance + const bool is_reverse = (target_vel < 0); + const double min_lookahead_distance = + is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; + const double lookahead_distance = calcLookaheadDistance( + current_odometry_->twist.twist.linear.x, param_.lookahead_distance_ratio, + min_lookahead_distance); + + // Set PurePursuit data + pure_pursuit_->setCurrentPose(current_pose_->pose); + pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_)); + pure_pursuit_->setLookaheadDistance(lookahead_distance); + + // Run PurePursuit + const auto pure_pursuit_result = pure_pursuit_->run(); + if (!pure_pursuit_result.first) { + return {}; + } + + const auto kappa = pure_pursuit_result.second; + + // Set debug data + debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); + + return kappa; +} + +boost::optional PurePursuitNode::calcTargetPoint() + const +{ + const auto closest_idx_result = planning_utils::findClosestIdxWithDistAngThr( + planning_utils::extractPoses(*trajectory_), current_pose_->pose, 3.0, M_PI_4); + + if (!closest_idx_result.first) { + RCLCPP_ERROR(get_logger(), "cannot find closest waypoint"); + return {}; + } + + return trajectory_->points.at(closest_idx_result.second); +} +} // namespace autoware::pure_pursuit + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pure_pursuit::PurePursuitNode) diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_viz.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_viz.cpp new file mode 100644 index 0000000000000..0a2f5fd92a0eb --- /dev/null +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_viz.cpp @@ -0,0 +1,93 @@ +// 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. + +/* + * Copyright 2015-2019 Autoware Foundation. 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 "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp" + +#include "autoware_pure_pursuit/util/marker_helper.hpp" +#include "autoware_pure_pursuit/util/planning_utils.hpp" + +#include + +namespace +{ +std::vector generateTrajectoryCircle( + const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose) +{ + constexpr double theta_range = M_PI / 10; + constexpr double step_rad = 0.005; + + const double radius = autoware::pure_pursuit::planning_utils::calcRadius(target, current_pose); + + std::vector trajectory_circle; + for (double theta = -theta_range; theta <= theta_range; theta += step_rad) { + geometry_msgs::msg::Point p; + p.x = radius * sin(theta); + p.y = radius * (1 - cos(theta)); + p.z = target.z; + + trajectory_circle.push_back( + autoware::pure_pursuit::planning_utils::transformToAbsoluteCoordinate2D(p, current_pose)); + } + + return trajectory_circle; +} + +} // namespace + +namespace autoware::pure_pursuit +{ +visualization_msgs::msg::Marker createNextTargetMarker( + const geometry_msgs::msg::Point & next_target) +{ + auto marker = createDefaultMarker( + "map", "next_target", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(0.0, 1.0, 0.0, 1.0)); + + marker.pose.position = next_target; + + return marker; +} + +visualization_msgs::msg::Marker createTrajectoryCircleMarker( + const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose) +{ + auto marker = createDefaultMarker( + "map", "trajectory_circle", 0, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.05, 0, 0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + + const auto trajectory_circle = generateTrajectoryCircle(target, current_pose); + for (auto p : trajectory_circle) { + marker.points.push_back(p); + marker.colors.push_back(marker.color); + } + + return marker; +} +} // namespace autoware::pure_pursuit diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp new file mode 100644 index 0000000000000..25a3334eb26f8 --- /dev/null +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/autoware_pure_pursuit.cpp @@ -0,0 +1,212 @@ +// 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. +/* + * Copyright 2015-2019 Autoware Foundation. 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 "autoware_pure_pursuit/autoware_pure_pursuit.hpp" + +#include "autoware_pure_pursuit/util/planning_utils.hpp" + +#include +#include +#include +#include + +namespace autoware::pure_pursuit +{ +bool PurePursuit::isDataReady() +{ + if (!curr_wps_ptr_) { + return false; + } + if (!curr_pose_ptr_) { + return false; + } + return true; +} + +std::pair PurePursuit::run() +{ + if (!isDataReady()) { + return std::make_pair(false, std::numeric_limits::quiet_NaN()); + } + + auto closest_pair = planning_utils::findClosestIdxWithDistAngThr( + *curr_wps_ptr_, *curr_pose_ptr_, closest_thr_dist_, closest_thr_ang_); + + if (!closest_pair.first) { + RCLCPP_WARN( + logger, "cannot find, curr_bool: %d, closest_idx: %d", closest_pair.first, + closest_pair.second); + return std::make_pair(false, std::numeric_limits::quiet_NaN()); + } + + int32_t next_wp_idx = findNextPointIdx(closest_pair.second); + if (next_wp_idx == -1) { + RCLCPP_WARN(logger, "lost next waypoint"); + return std::make_pair(false, std::numeric_limits::quiet_NaN()); + } + + loc_next_wp_ = curr_wps_ptr_->at(next_wp_idx).position; + + geometry_msgs::msg::Point next_tgt_pos; + // if next waypoint is first + if (next_wp_idx == 0) { + next_tgt_pos = curr_wps_ptr_->at(next_wp_idx).position; + } else { + // linear interpolation + std::pair lerp_pair = lerpNextTarget(next_wp_idx); + + if (!lerp_pair.first) { + RCLCPP_WARN(logger, "lost target! "); + return std::make_pair(false, std::numeric_limits::quiet_NaN()); + } + + next_tgt_pos = lerp_pair.second; + } + loc_next_tgt_ = next_tgt_pos; + + double kappa = planning_utils::calcCurvature(next_tgt_pos, *curr_pose_ptr_); + + return std::make_pair(true, kappa); +} + +// linear interpolation of next target +std::pair PurePursuit::lerpNextTarget(int32_t next_wp_idx) +{ + constexpr double ERROR2 = 1e-5; // 0.00001 + const geometry_msgs::msg::Point & vec_end = curr_wps_ptr_->at(next_wp_idx).position; + const geometry_msgs::msg::Point & vec_start = curr_wps_ptr_->at(next_wp_idx - 1).position; + const geometry_msgs::msg::Pose & curr_pose = *curr_pose_ptr_; + + Eigen::Vector3d vec_a( + (vec_end.x - vec_start.x), (vec_end.y - vec_start.y), (vec_end.z - vec_start.z)); + + if (vec_a.norm() < ERROR2) { + RCLCPP_ERROR(logger, "waypoint interval is almost 0"); + return std::make_pair(false, geometry_msgs::msg::Point()); + } + + const double lateral_error = + planning_utils::calcLateralError2D(vec_start, vec_end, curr_pose.position); + + if (fabs(lateral_error) > lookahead_distance_) { + RCLCPP_ERROR(logger, "lateral error is larger than lookahead distance"); + RCLCPP_ERROR( + logger, "lateral error: %lf, lookahead distance: %lf", lateral_error, lookahead_distance_); + return std::make_pair(false, geometry_msgs::msg::Point()); + } + + /* calculate the position of the foot of a perpendicular line */ + Eigen::Vector2d uva2d(vec_a.x(), vec_a.y()); + uva2d.normalize(); + Eigen::Rotation2Dd rot = + (lateral_error > 0) ? Eigen::Rotation2Dd(-M_PI / 2.0) : Eigen::Rotation2Dd(M_PI / 2.0); + Eigen::Vector2d uva2d_rot = rot * uva2d; + + geometry_msgs::msg::Point h; + h.x = curr_pose.position.x + fabs(lateral_error) * uva2d_rot.x(); + h.y = curr_pose.position.y + fabs(lateral_error) * uva2d_rot.y(); + h.z = curr_pose.position.z; + + // if there is a intersection + if (fabs(fabs(lateral_error) - lookahead_distance_) < ERROR2) { + return std::make_pair(true, h); + } else { + // if there are two intersection + // get intersection in front of vehicle + const double s = sqrt(pow(lookahead_distance_, 2) - pow(lateral_error, 2)); + geometry_msgs::msg::Point res; + res.x = h.x + s * uva2d.x(); + res.y = h.y + s * uva2d.y(); + res.z = curr_pose.position.z; + return std::make_pair(true, res); + } +} + +int32_t PurePursuit::findNextPointIdx(int32_t search_start_idx) +{ + // if waypoints are not given, do nothing. + if (curr_wps_ptr_->empty() || search_start_idx == -1) { + return -1; + } + + // look for the next waypoint. + for (int32_t i = search_start_idx; i < (int32_t)curr_wps_ptr_->size(); i++) { + // if search waypoint is the last + if (i == ((int32_t)curr_wps_ptr_->size() - 1)) { + return i; + } + + // if waypoint direction is forward + const auto gld = planning_utils::getLaneDirection(*curr_wps_ptr_, 0.05); + if (gld == 0) { + // if waypoint is not in front of ego, skip + auto ret = planning_utils::transformToRelativeCoordinate2D( + curr_wps_ptr_->at(i).position, *curr_pose_ptr_); + if (ret.x < 0) { + continue; + } + } else if (gld == 1) { + // waypoint direction is backward + + // if waypoint is in front of ego, skip + auto ret = planning_utils::transformToRelativeCoordinate2D( + curr_wps_ptr_->at(i).position, *curr_pose_ptr_); + if (ret.x > 0) { + continue; + } + } else { + return -1; + } + + const geometry_msgs::msg::Point & curr_motion_point = curr_wps_ptr_->at(i).position; + const geometry_msgs::msg::Point & curr_pose_point = curr_pose_ptr_->position; + // if there exists an effective waypoint + const double ds = planning_utils::calcDistSquared2D(curr_motion_point, curr_pose_point); + if (ds > std::pow(lookahead_distance_, 2)) { + return i; + } + } + + // if this program reaches here , it means we lost the waypoint! + return -1; +} + +void PurePursuit::setCurrentPose(const geometry_msgs::msg::Pose & msg) +{ + curr_pose_ptr_ = std::make_shared(); + *curr_pose_ptr_ = msg; +} + +void PurePursuit::setWaypoints(const std::vector & msg) +{ + curr_wps_ptr_ = std::make_shared>(); + *curr_wps_ptr_ = msg; +} + +} // namespace autoware::pure_pursuit diff --git a/control/pure_pursuit/src/pure_pursuit_core/interpolate.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp similarity index 98% rename from control/pure_pursuit/src/pure_pursuit_core/interpolate.cpp rename to control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp index 8890e0c7da25d..8d7078c87cf39 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/interpolate.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/interpolate.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pure_pursuit/util/interpolate.hpp" +#include "autoware_pure_pursuit/util/interpolate.hpp" #include #include -namespace pure_pursuit +namespace autoware::pure_pursuit { bool LinearInterpolate::interpolate( const std::vector & base_index, const std::vector & base_value, @@ -234,4 +234,4 @@ bool SplineInterpolate::interpolate( } return true; } -} // namespace pure_pursuit +} // namespace autoware::pure_pursuit diff --git a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp similarity index 98% rename from control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp rename to control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp index b3a846157638a..bed6fafcf5bfd 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pure_pursuit/util/planning_utils.hpp" +#include "autoware_pure_pursuit/util/planning_utils.hpp" #include #include #include -namespace pure_pursuit +namespace autoware::pure_pursuit { namespace planning_utils { @@ -266,4 +266,4 @@ geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double _yaw) } } // namespace planning_utils -} // namespace pure_pursuit +} // namespace autoware::pure_pursuit diff --git a/control/autoware_shift_decider/CMakeLists.txt b/control/autoware_shift_decider/CMakeLists.txt new file mode 100644 index 0000000000000..269b0fcc33690 --- /dev/null +++ b/control/autoware_shift_decider/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_shift_decider) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/${PROJECT_NAME}.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "autoware::shift_decider::ShiftDecider" + EXECUTABLE ${PROJECT_NAME} +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_shift_decider/README.md b/control/autoware_shift_decider/README.md new file mode 100644 index 0000000000000..84767abceff6b --- /dev/null +++ b/control/autoware_shift_decider/README.md @@ -0,0 +1,56 @@ +# Shift Decider + +## Purpose + +`autoware_shift_decider` is a module to decide shift from ackermann control command. + +## Inner-workings / Algorithms + +### Flow chart + +```plantuml +@startuml +skinparam monochrome true + +title update current shift +start +if (absolute target velocity is less than threshold) then (yes) + :set previous shift; +else(no) +if (target velocity is positive) then (yes) + :set shift DRIVE; +else + :set shift REVERSE; +endif +endif + :publish current shift; +note right + publish shift for constant interval +end note +stop +@enduml +``` + +### Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| --------------------- | ------------------------------------- | ---------------------------- | +| `~/input/control_cmd` | `autoware_control_msgs::msg::Control` | Control command for vehicle. | + +### Output + +| Name | Type | Description | +| ------------------ | ----------------------------------------- | ---------------------------------- | +| `~output/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | + +## Parameters + +none. + +## Assumptions / Known limits + +TBD. diff --git a/control/shift_decider/config/shift_decider.param.yaml b/control/autoware_shift_decider/config/shift_decider.param.yaml similarity index 100% rename from control/shift_decider/config/shift_decider.param.yaml rename to control/autoware_shift_decider/config/shift_decider.param.yaml diff --git a/control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp b/control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp new file mode 100644 index 0000000000000..4702ef17e49bf --- /dev/null +++ b/control/autoware_shift_decider/include/autoware_shift_decider/autoware_shift_decider.hpp @@ -0,0 +1,65 @@ +// 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 AUTOWARE_SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ +#define AUTOWARE_SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ + +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" + +#include + +#include +#include +#include +#include + +#include + +namespace autoware::shift_decider +{ + +class ShiftDecider : public rclcpp::Node +{ +public: + explicit ShiftDecider(const rclcpp::NodeOptions & node_options); + +private: + void onTimer(); + void onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg); + void onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg); + void onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg); + void updateCurrentShiftCmd(); + void initTimer(double period_s); + + rclcpp::Publisher::SharedPtr pub_shift_cmd_; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_control_cmd_{this, "input/control_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_autoware_state_{this, "input/state"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_current_gear_{this, "input/current_gear"}; + + rclcpp::TimerBase::SharedPtr timer_; + + autoware_control_msgs::msg::Control::ConstSharedPtr control_cmd_; + autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_; + autoware_vehicle_msgs::msg::GearCommand shift_cmd_; + autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr current_gear_ptr_; + uint8_t prev_shift_command = autoware_vehicle_msgs::msg::GearCommand::PARK; + + bool park_on_goal_; +}; +} // namespace autoware::shift_decider + +#endif // AUTOWARE_SHIFT_DECIDER__AUTOWARE_SHIFT_DECIDER_HPP_ diff --git a/control/autoware_shift_decider/launch/shift_decider.launch.xml b/control/autoware_shift_decider/launch/shift_decider.launch.xml new file mode 100644 index 0000000000000..d04af22ec828d --- /dev/null +++ b/control/autoware_shift_decider/launch/shift_decider.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/control/autoware_shift_decider/package.xml b/control/autoware_shift_decider/package.xml new file mode 100644 index 0000000000000..40708c2c73cf1 --- /dev/null +++ b/control/autoware_shift_decider/package.xml @@ -0,0 +1,30 @@ + + + + autoware_shift_decider + 0.1.0 + The autoware_shift_decider package + Takamasa Horibe + Takayuki Murooka + Apache License 2.0 + + Takamasa Horibe + + ament_cmake + autoware_cmake + + autoware_control_msgs + autoware_system_msgs + autoware_vehicle_msgs + rclcpp + rclcpp_components + tier4_autoware_utils + + ament_cmake_cppcheck + ament_cmake_cpplint + ament_lint_auto + + + ament_cmake + + diff --git a/control/shift_decider/schema/shift_decider.schema.json b/control/autoware_shift_decider/schema/shift_decider.schema.json similarity index 100% rename from control/shift_decider/schema/shift_decider.schema.json rename to control/autoware_shift_decider/schema/shift_decider.schema.json diff --git a/control/autoware_shift_decider/src/autoware_shift_decider.cpp b/control/autoware_shift_decider/src/autoware_shift_decider.cpp new file mode 100644 index 0000000000000..91345376c2681 --- /dev/null +++ b/control/autoware_shift_decider/src/autoware_shift_decider.cpp @@ -0,0 +1,93 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_shift_decider/autoware_shift_decider.hpp" + +#include + +#include +#include + +namespace autoware::shift_decider +{ + +ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) +: Node("shift_decider", node_options) +{ + using std::placeholders::_1; + + static constexpr std::size_t queue_size = 1; + rclcpp::QoS durable_qos(queue_size); + durable_qos.transient_local(); + + park_on_goal_ = declare_parameter("park_on_goal"); + + pub_shift_cmd_ = + create_publisher("output/gear_cmd", durable_qos); + + initTimer(0.1); +} + +void ShiftDecider::onTimer() +{ + control_cmd_ = sub_control_cmd_.takeData(); + autoware_state_ = sub_autoware_state_.takeData(); + current_gear_ptr_ = sub_current_gear_.takeData(); + if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) { + return; + } + + updateCurrentShiftCmd(); + pub_shift_cmd_->publish(shift_cmd_); +} + +void ShiftDecider::updateCurrentShiftCmd() +{ + using autoware_system_msgs::msg::AutowareState; + using autoware_vehicle_msgs::msg::GearCommand; + + shift_cmd_.stamp = now(); + static constexpr double vel_threshold = 0.01; // to prevent chattering + if (autoware_state_->state == AutowareState::DRIVING) { + if (control_cmd_->longitudinal.velocity > vel_threshold) { + shift_cmd_.command = GearCommand::DRIVE; + } else if (control_cmd_->longitudinal.velocity < -vel_threshold) { + shift_cmd_.command = GearCommand::REVERSE; + } else { + shift_cmd_.command = prev_shift_command; + } + } else { + if ( + (autoware_state_->state == AutowareState::ARRIVED_GOAL || + autoware_state_->state == AutowareState::WAITING_FOR_ROUTE) && + park_on_goal_) { + shift_cmd_.command = GearCommand::PARK; + } else { + shift_cmd_.command = current_gear_ptr_->report; + } + } + prev_shift_command = shift_cmd_.command; +} + +void ShiftDecider::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(&ShiftDecider::onTimer, this)); +} +} // namespace autoware::shift_decider + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::shift_decider::ShiftDecider) diff --git a/control/smart_mpc_trajectory_follower/.gitignore b/control/autoware_smart_mpc_trajectory_follower/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/.gitignore diff --git a/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt new file mode 100644 index 0000000000000..57e7b596790c2 --- /dev/null +++ b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_smart_mpc_trajectory_follower) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +install(PROGRAMS + autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py + DESTINATION lib/${PROJECT_NAME} +) +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/control/autoware_smart_mpc_trajectory_follower/README.md b/control/autoware_smart_mpc_trajectory_follower/README.md new file mode 100644 index 0000000000000..fd135319ff900 --- /dev/null +++ b/control/autoware_smart_mpc_trajectory_follower/README.md @@ -0,0 +1,338 @@ +

+ + + +

+ + +# Smart MPC Trajectory Follower + +Smart MPC (Model Predictive Control) is a control algorithm that combines model predictive control and machine learning. While inheriting the advantages of model predictive control, it solves its disadvantage of modeling difficulty with a data-driven method using machine learning. + +This technology makes it relatively easy to operate model predictive control, which is expensive to implement, as long as an environment for collecting data can be prepared. + +

+ + + +

+ +## Setup + +After building autoware, move to `control/autoware_smart_mpc_trajectory_follower` and run the following command: + +```bash +pip3 install . +``` + +If you have already installed and want to update the package, run the following command instead: + +```bash +pip3 install -U . +``` + +## Provided features + +This package provides smart MPC logic for path-following control as well as mechanisms for learning and evaluation. These features are described below. + +### Trajectory following control based on iLQR/MPPI + +The control mode can be selected from "ilqr", "mppi", or "mppi_ilqr", and can be set as `mpc_parameter:system:mode` in [mpc_param.yaml](./autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml). +In "mppi_ilqr" mode, the initial value of iLQR is given by the MPPI solution. + +> [!NOTE] +> With the default settings, the performance of "mppi" mode is limited due to an insufficient number of samples. This issue is being addressed with ongoing work to introduce GPU support. + +To perform a simulation, run the following command: + +```bash +ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit trajectory_follower_mode:=smart_mpc_trajectory_follower +``` + +> [!NOTE] +> When running with the nominal model set in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml), set `trained_model_parameter:control_application:use_trained_model` to `false` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). To run using the trained model, set `trained_model_parameter:control_application:use_trained_model` to `true`, but the trained model must have been generated according to the following procedure. + +### Training of model and reflection in control + +To obtain training data, start autoware, perform a drive, and record rosbag data with the following commands. + +```bash +ros2 bag record /localization/kinematic_state /localization/acceleration /vehicle/status/steering_status /control/command/control_cmd /control/trajectory_follower/control_cmd /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw /system/operation_mode/state /vehicle/status/control_mode /sensing/imu/imu_data /debug_mpc_x_des /debug_mpc_y_des /debug_mpc_v_des /debug_mpc_yaw_des /debug_mpc_acc_des /debug_mpc_steer_des /debug_mpc_X_des_converted /debug_mpc_x_current /debug_mpc_error_prediction /debug_mpc_max_trajectory_err /debug_mpc_emergency_stop_mode /debug_mpc_goal_stop_mode /debug_mpc_total_ctrl_time /debug_mpc_calc_u_opt_time +``` + +Move [rosbag2.bash](./autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash) to the rosbag directory recorded above and execute the following command on the directory + +```bash +bash rosbag2.bash +``` + +This converts rosbag data into CSV format for training models. + +> [!NOTE] +> Note that a large number of terminals are automatically opened at runtime, but they are automatically closed after rosbag data conversion is completed. +> From the time you begin this process until all terminals are closed, autoware should not be running. + +Instead, the same result can be obtained by executing the following command in a python environment: + +```python +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +model_trainer = train_drive_NN_model.train_drive_NN_model() +model_trainer.transform_rosbag_to_csv(rosbag_dir) +``` + +Here, `rosbag_dir` represents the rosbag directory. +At this time, all CSV files in `rosbag_dir` are automatically deleted first. + +The paths of the rosbag directories used for training, `dir_0`, `dir_1`, `dir_2`,... and the directory `save_dir` where you save the models, the model can be saved in the python environment as follows: + +```python +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +model_trainer = train_drive_NN_model.train_drive_NN_model() +model_trainer.add_data_from_csv(dir_0) +model_trainer.add_data_from_csv(dir_1) +model_trainer.add_data_from_csv(dir_2) +... +model_trainer.get_trained_model() +model_trainer.save_models(save_dir) +``` + +After performing the polynomial regression, the NN can be trained on the residuals as follows: + +```python +model_trainer.get_trained_model(use_polynomial_reg=True) +``` + +> [!NOTE] +> In the default setting, regression is performed by several preselected polynomials. +> When `use_selected_polynomial=False` is set as the argument of get_trained_model, the `deg` argument allows setting the maximum degree of the polynomial to be used. + +If only polynomial regression is performed and no NN model is used, run the following command: + +```python +model_trainer.get_trained_model(use_polynomial_reg=True,force_NN_model_to_zero=True) +``` + +Move `model_for_test_drive.pth` and `polynomial_reg_info.npz` saved in `save_dir` to the home directory and set `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) to `true` to reflect the trained model in the control. + +### Performance evaluation + +Here, as an example, we describe the verification of the adaptive performance when the wheel base of the sample_vehicle is 2.79 m, but an incorrect value of 2.0 m is given to the controller side. +To give the controller 2.0 m as the wheel base, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.0, and run the following command: + +```bash +python3 -m smart_mpc_trajectory_follower.clear_pycache +``` + +#### Test on autoware + +To perform a control test on autoware with the nominal model before training, make sure that `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) is `false` and launch autoware in the manner described in "Trajectory following control based on iLQR/MPPI". This time, the following route will be used for the test: + +

+ +Record rosbag and train the model in the manner described in "Training of model and reflection in control", and move the generated files `model_for_test_drive.pth` and `polynomial_reg_info.npz` to the home directory. + +> [!NOTE] +> Although the data used for training is small, for the sake of simplicity, we will see how much performance can be improved with this amount of data. + +To control using the trained model obtained here, set `trained_model_parameter:control_application:use_trained_model` to `true`, start autoware in the same way, and drive the same route recording rosbag. +After the driving is complete, convert the rosbag file to CSV format using the method described in "Training of model and reflection in control". +A plot of the lateral deviation is obtained by running the `lateral_error_visualize` function in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb` for the nominal and training model rosbag files `rosbag_nominal` and `rosbag_trained`, respectively, as follows: + +```python +lateral_error_visualize(dir_name=rosbag_nominal,ylim=[-1.2,1.2]) +lateral_error_visualize(dir_name=rosbag_trained,ylim=[-1.2,1.2]) +``` + +The following results were obtained. + +
+ + +
+ +#### Test on python simulator + +First, to give wheel base 2.79 m in the python simulator, create the following file and save it in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator` with the name `sim_setting.json`: + +```json +{ "wheel_base": 2.79 } +``` + +Next, run the following commands to test the slalom driving on the python simulator with the nominal model: + +```python +import python_simulator +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +initial_error = [0.0, 0.03, 0.01, -0.01, 0.0, 0.0] +save_dir = "test_python_sim" +python_simulator.slalom_drive(save_dir=save_dir,use_trained_model=False,initial_error=initial_error) +``` + +Here, `initial_error` is the initial error from the target trajectory, in the order of x-coordinate, y-coordinate, longitudinal velocity, yaw angle, longitudinal acceleration, and steer angle, +and `save_dir` is the directory where the driving test results are saved. + +> [!NOTE] +> The value of `use_trained_model` given as the argument of `python_simulator.slalom_drive` takes precedence over the value of `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). + +Run the following commands to perform training using driving data of the nominal model. + +```python +model_trainer = train_drive_NN_model.train_drive_NN_model() +model_trainer.add_data_from_csv(save_dir) +model_trainer.save_train_data(save_dir) +model_trainer.get_trained_model(use_polynomial_reg=True) +model_trainer.save_models(save_dir=save_dir) +``` + +This way, files `model_for_test_drive.pth` and `polynomial_reg_info.npz` are saved in `save_dir`. +The following results were obtained. + +

+ +

+ +The center of the upper row represents the lateral deviation. + +Finally, to drive with the training model, run the following commands: + +```python +load_dir = save_dir +save_dir = "test_python_trained_sim" +python_simulator.slalom_drive(save_dir=save_dir,load_dir=load_dir,use_trained_model=True,initial_error=initial_error) +``` + +The following results were obtained. + +

+ +

+ +It can be seen that the lateral deviation has improved significantly. + +Here we have described wheel base, but the parameters that can be passed to the python simulator are as follows. + +| Parameter | Type | Description | +| ------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| steer_bias | float | steer bias [rad] | +| steer_rate_lim | float | steer rate limit [rad/s] | +| vel_rate_lim | float | acceleration limit [m/s^2] | +| wheel_base | float | wheel base [m] | +| steer_dead_band | float | steer dead band [rad] | +| adaptive_gear_ratio_coef | list[float] | List of floats of length 6 specifying information on speed-dependent gear ratios from tire angle to steering wheel angle. | +| acc_time_delay | float | acceleration time delay [s] | +| steer_time_delay | float | steer time delay [s] | +| acc_time_constant | float | acceleration time constant [s] | +| steer_time_constant | float | steer time constant [s] | +| accel_map_scale | float | Parameter that magnifies the corresponding distortion from acceleration input values to actual acceleration realizations.
Correspondence information is kept in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv`. | +| acc_scaling | float | acceleration scaling | +| steer_scaling | float | steer scaling | + +For example, to give the simulation side 0.01 [rad] of steer bias and 0.001 [rad] of steer dead band, edit the `sim_setting.json` as follows. + +```json +{ "steer_bias": 0.01, "steer_dead_band": 0.001 } +``` + +#### Auto test on python simulator + +Here, we describe a method for testing adaptive performance by giving the simulation side a predefined range of model parameters while the control side is given constant model parameters. + +First, to restore nominal model settings to default values, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.79, and run the following command: + +```bash +python3 -m smart_mpc_trajectory_follower.clear_pycache +``` + +To run a driving experiment within the parameter change range set in [run_sim.py](./autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py), for example, move to `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator` and run the following command: + +```bash +python3 run_sim.py --param_name steer_bias +``` + +Here we described the experimental procedure for steer bias, and the same method can be used for other parameters. + +If you want to do it for all parameters at once, run the following command: + +```bash +yes | python3 run_sim.py +``` + +In `run_sim.py`, the following parameters can be set: + +| Parameter | Type | Description | +| ------------------------- | ---- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| USE_TRAINED_MODEL_DIFF | bool | Whether the derivative of the trained model is reflected in the control | +| DATA_COLLECTION_MODE | str | Which method will be used to collect the training data　
"ff": Straight line driving with feed-forward input
"pp": Figure eight driving with pure pursuit control
"mpc": Slalom driving with mpc | +| USE_POLYNOMIAL_REGRESSION | bool | Whether to perform polynomial regression before NN | +| USE_SELECTED_POLYNOMIAL | bool | When USE_POLYNOMIAL_REGRESSION is True, perform polynomial regression using only some preselected polynomials.
The choice of polynomials is intended to be able to absorb the contribution of some parameter shifts based on the nominal model of the vehicle. | +| FORCE_NN_MODEL_TO_ZERO | bool | Whether to force the NN model to zero (i.e., erase the contribution of the NN model).
When USE_POLYNOMIAL_REGRESSION is True, setting FORCE_MODEL_TO_ZERO to True allows the control to reflect the results of polynomial regression only, without using NN models. | +| FIT_INTERCEPT | bool | Whether to include bias in polynomial regression.
If it is False, perform the regression with a polynomial of the first degree or higher. | +| USE_INTERCEPT | bool | When a polynomial regression including bias is performed, whether to use or discard the resulting bias information.
It is meaningful only if FIT_INTERCEPT is True.
If it is False, discard the bias in the polynomial regression in the hope that the NN model can remove the bias term, even if the polynomial regression is performed with the bias included. | + +> [!NOTE] +> When `run_sim.py` is run, the `use_trained_model_diff` set in `run_sim.py` takes precedence over the `trained_model_parameter:control_application:use_trained_model_diff` set in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). + +## Change of nominal parameters and their reloading + +The nominal parameters of vehicle model can be changed by editing the file [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml). +After changing the nominal parameters, the cache must be deleted by running the following command: + +```bash +python3 -m smart_mpc_trajectory_follower.clear_pycache +``` + +The nominal parameters include the following: + +| Parameter | Type | Description | +| ------------------------------------------------ | ----- | ------------------------------ | +| nominal_parameter:vehicle_info:wheel_base | float | wheel base [m] | +| nominal_parameter:acceleration:acc_time_delay | float | acceleration time delay [s] | +| nominal_parameter:acceleration:acc_time_constant | float | acceleration time constant [s] | +| nominal_parameter:steering:steer_time_delay | float | steer time delay [s] | +| nominal_parameter:steering:steer_time_constant | float | steer time constant [s] | + +## Change of control parameters and their reloading + +The control parameters can be changed by editing files [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml) and [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). +Although it is possible to reflect parameter changes by restarting autoware, the following command allows us to do so without leaving autoware running: + +```bash +ros2 topic pub /pympc_reload_mpc_param_trigger std_msgs/msg/String "data: ''" --once +``` + +The main parameters among the control parameters are as follows. + +### `mpc_param.yaml` + +| Parameter | Type | Description | +| ------------------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| mpc_parameter:system:mode | str | control mode
"ilqr": iLQR mode
"mppi": MPPI mode
"mppi_ilqr": the initial value of iLQR is given by the MPPI solution. | +| mpc_parameter:cost_parameters:Q | list[float] | Stage cost for states.
List of length 8, in order: straight deviation, lateral deviation, velocity deviation, yaw angle deviation, acceleration deviation, steer deviation, acceleration input deviation, steer input deviation cost weights. | +| mpc_parameter:cost_parameters:Q_c | list[float] | Cost in the horizon corresponding to the following timing_Q_c for the states.
The correspondence of the components of the list is the same as for Q. | +| mpc_parameter:cost_parameters:Q_f | list[float] | Termination cost for the states.
The correspondence of the components of the list is the same as for Q. | +| mpc_parameter:cost_parameters:R | list[float] | A list of length 2 where R[0] is weight of cost for the change rate of acceleration input value and R[1] is weight of cost for the change rate of steer input value. | +| mpc_parameter:mpc_setting:timing_Q_c | list[int] | Horizon numbers such that the stage cost for the states is set to Q_c. | + +### `trained_model_param.yaml` + +| Parameter | Type | Description | +| ------------------------------------------------------------------ | ---- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| trained_model_parameter:control_application:use_trained_model | bool | Whether the trained model is reflected in the control or not. | +| trained_model_parameter:control_application:use_trained_model_diff | bool | Whether the derivative of the trained model is reflected on the control or not.
It is meaningful only when use_trained_model is True, and if False, the nominal model is used for the derivative of the dynamics, and trained model is used only for prediction. | + +## Request to release the slow stop mode + +If the predicted trajectory deviates too far from the target trajectory, the system enters a slow stop mode and the vehicle stops moving. +To cancel the slow stop mode and make the vehicle ready to run again, run the following command: + +```bash +ros2 topic pub /pympc_stop_mode_reset_request std_msgs/msg/String "data: ''" --once +``` + +## Limitation + +- May not be able to start when initial position/posture is far from the target. + +- It may take some time until the end of the planning to compile numba functions at the start of the first control. + +- In the stopping action near the goal our control switches to another simple control law. As a result, the stopping action may not work except near the goal. Stopping is also difficult if the acceleration map is significantly shifted. diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py similarity index 88% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py index fafcb5bdecf05..2458de3a80f99 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py @@ -16,10 +16,10 @@ from pathlib import Path import shutil -import smart_mpc_trajectory_follower +import autoware_smart_mpc_trajectory_follower if __name__ == "__main__": - package_dir = str(Path(smart_mpc_trajectory_follower.__file__).parent) + package_dir = str(Path(autoware_smart_mpc_trajectory_follower.__file__).parent) remove_dirs = [ package_dir + "/__pycache__", diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/mpc_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/mpc_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/nominal_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/nominal_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/trained_model_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/trained_model_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py index d706c342f04ec..0eaa6ce1c0fbf 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py @@ -18,14 +18,14 @@ import datetime import os +from autoware_smart_mpc_trajectory_follower.scripts import drive_controller +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions import matplotlib.pyplot as plt from numba import njit import numpy as np import pandas as pd import scipy.interpolate import simplejson as json -from smart_mpc_trajectory_follower.scripts import drive_controller -from smart_mpc_trajectory_follower.scripts import drive_functions print("\n\n### import python_simulator.py ###") diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py similarity index 92% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py index 6b477bb804ac9..68d8a9f64e73f 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model import numpy as np import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model initial_error = np.array( [0.001, 0.03, 0.01, -0.001, 0, 2 * python_simulator.measurement_steer_bias] diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py index 3125719556559..d2bb4ea26f0a6 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py @@ -23,9 +23,9 @@ import traceback from typing import Dict +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model import numpy as np import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model parser = argparse.ArgumentParser() parser.add_argument("--param_name", default=None) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py index e75eedcb4bf65..4d36afd3d4324 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py @@ -16,9 +16,9 @@ from functools import partial +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions sqrt_mpc_time_step = np.sqrt(drive_functions.mpc_time_step) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py index f35789ee39fd3..f13aa07b0f795 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py @@ -13,9 +13,9 @@ # limitations under the License. +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.scripts import proxima_calc import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.scripts import proxima_calc import torch from torch import nn diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py index b21fcf72da931..228b0c37c5e7b 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py @@ -21,14 +21,14 @@ import threading import time +from autoware_smart_mpc_trajectory_follower.scripts import drive_GP +from autoware_smart_mpc_trajectory_follower.scripts import drive_NN +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.scripts import drive_iLQR +from autoware_smart_mpc_trajectory_follower.scripts import drive_mppi import numpy as np import scipy.interpolate # type: ignore from sklearn.preprocessing import PolynomialFeatures -from smart_mpc_trajectory_follower.scripts import drive_GP -from smart_mpc_trajectory_follower.scripts import drive_NN -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.scripts import drive_iLQR -from smart_mpc_trajectory_follower.scripts import drive_mppi import torch ctrl_index_for_polynomial_reg = np.concatenate( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py index c11b5c886b49a..fb96e134f7746 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py @@ -31,7 +31,9 @@ with open(package_path_json, "r") as file: package_path = json.load(file) -mpc_param_path = package_path["path"] + "/smart_mpc_trajectory_follower/param/mpc_param.yaml" +mpc_param_path = ( + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml" +) with open(mpc_param_path, "r") as yml: mpc_param = yaml.safe_load(yml) @@ -101,7 +103,7 @@ cap_pred_error = np.array(mpc_param["mpc_parameter"]["preprocessing"]["cap_pred_error"]) nominal_param_path = ( - package_path["path"] + "/smart_mpc_trajectory_follower/param/nominal_param.yaml" + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml" ) with open(nominal_param_path, "r") as yml: nominal_param = yaml.safe_load(yml) @@ -172,7 +174,7 @@ mpc_param["mpc_parameter"]["preprocessing"]["sg_window_size_for_nominal_inputs"] ) trained_model_param_path = ( - package_path["path"] + "/smart_mpc_trajectory_follower/param/trained_model_param.yaml" + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml" ) with open(trained_model_param_path, "r") as yml: trained_model_param = yaml.safe_load(yml) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py index 261a72e680778..2307586f23552 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py @@ -21,9 +21,9 @@ import time from typing import Callable +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions index_cost = np.concatenate( ( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py index ca6e6f15f42ef..5e88c79172097 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py @@ -16,9 +16,9 @@ from typing import Callable +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions index_cost = np.concatenate( ( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/proxima_calc.cpp b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/proxima_calc.cpp rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index 992a5fd53a10c..78a120fe601f2 100755 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -22,6 +22,8 @@ from autoware_control_msgs.msg import Control from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint +from autoware_smart_mpc_trajectory_follower.scripts import drive_controller +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from autoware_vehicle_msgs.msg import SteeringReport from builtin_interfaces.msg import Duration from geometry_msgs.msg import AccelWithCovarianceStamped @@ -34,8 +36,6 @@ import scipy.interpolate from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Slerp -from smart_mpc_trajectory_follower.scripts import drive_controller -from smart_mpc_trajectory_follower.scripts import drive_functions from std_msgs.msg import String from tier4_debug_msgs.msg import BoolStamped from tier4_debug_msgs.msg import Float32MultiArrayStamped diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py index 3f6ef7a9f78d1..5338f3cbaddce 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py @@ -23,11 +23,11 @@ import os from pathlib import Path +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions import numpy as np import scipy.interpolate from scipy.ndimage import gaussian_filter from scipy.spatial.transform import Rotation -from smart_mpc_trajectory_follower.scripts import drive_functions def data_smoothing(data: np.ndarray, sigma: float) -> np.ndarray: @@ -76,7 +76,7 @@ def transform_rosbag_to_csv(self, dir_name: str, delete_csv_first: bool = True) os.system( "cp " + package_path["path"] - + "/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash " + + "/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash " + dir_name ) os.chdir(dir_name) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py index 269b577f83c25..449ddda45f460 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py @@ -15,13 +15,15 @@ # cspell: ignore optim savez suptitle """Class for training neural nets from driving data.""" +from autoware_smart_mpc_trajectory_follower.scripts import drive_NN +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.training_and_data_check import ( + add_training_data_from_csv, +) import matplotlib.pyplot as plt import numpy as np from sklearn import linear_model from sklearn.preprocessing import PolynomialFeatures -from smart_mpc_trajectory_follower.scripts import drive_NN -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.training_and_data_check import add_training_data_from_csv import torch from torch import nn from torch.utils.data import DataLoader diff --git a/control/smart_mpc_trajectory_follower/images/autoware_smart_mpc.png b/control/autoware_smart_mpc_trajectory_follower/images/autoware_smart_mpc.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/autoware_smart_mpc.png rename to control/autoware_smart_mpc_trajectory_follower/images/autoware_smart_mpc.png diff --git a/control/smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png diff --git a/control/smart_mpc_trajectory_follower/images/lateral_error_trained_model.png b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/lateral_error_trained_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png diff --git a/control/smart_mpc_trajectory_follower/images/proxima_logo.png b/control/autoware_smart_mpc_trajectory_follower/images/proxima_logo.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/proxima_logo.png rename to control/autoware_smart_mpc_trajectory_follower/images/proxima_logo.png diff --git a/control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png diff --git a/control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png diff --git a/control/smart_mpc_trajectory_follower/images/test_route.png b/control/autoware_smart_mpc_trajectory_follower/images/test_route.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/test_route.png rename to control/autoware_smart_mpc_trajectory_follower/images/test_route.png diff --git a/control/autoware_smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml new file mode 100644 index 0000000000000..a05f06cd865ff --- /dev/null +++ b/control/autoware_smart_mpc_trajectory_follower/package.xml @@ -0,0 +1,46 @@ + + + + autoware_smart_mpc_trajectory_follower + 1.0.0 + Nodes to follow a trajectory by generating control commands using smart mpc + + + Masayuki Aino + Kosuke Takeuchi + Takamasa Horibe + Takayuki Murooka + + Apache License 2.0 + + Masayuki Aino + + ament_cmake_auto + ament_cmake_python + autoware_cmake + + autoware_adapi_v1_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs + motion_utils + pybind11_vendor + python3-scipy + rclcpp + rclcpp_components + tier4_autoware_utils + + ros2launch + + ament_cmake_ros + ament_index_python + ament_lint_auto + autoware_lint_common + autoware_testing + ros_testing + + + ament_cmake + + diff --git a/control/autoware_smart_mpc_trajectory_follower/setup.py b/control/autoware_smart_mpc_trajectory_follower/setup.py new file mode 100644 index 0000000000000..fee1d04e826c0 --- /dev/null +++ b/control/autoware_smart_mpc_trajectory_follower/setup.py @@ -0,0 +1,37 @@ +# cspell: ignore numba +import glob +import json +import os +from pathlib import Path + +from setuptools import find_packages +from setuptools import setup + +os.system("pip3 install numba==0.58.1 --force-reinstall") +os.system("pip3 install pybind11") +os.system("pip3 install GPy") +os.system("pip3 install torch") +package_path = {} +package_path["path"] = str(Path(__file__).parent) +with open("autoware_smart_mpc_trajectory_follower/package_path.json", "w") as f: + json.dump(package_path, f) +build_cpp_command = "g++ -Ofast -Wall -shared -std=c++11 -fPIC $(python3 -m pybind11 --includes) " +build_cpp_command += "autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp " +build_cpp_command += "-o autoware_smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) " +build_cpp_command += "-lrt -I/usr/include/eigen3" +os.system(build_cpp_command) + +so_path = ( + "scripts/" + + glob.glob("autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.*.so")[0].split("/")[ + -1 + ] +) +setup( + name="smart_mpc_trajectory_follower", + version="1.0.0", + packages=find_packages(), + package_data={ + "autoware_smart_mpc_trajectory_follower": ["package_path.json", so_path], + }, +) diff --git a/control/autoware_trajectory_follower_base/CMakeLists.txt b/control/autoware_trajectory_follower_base/CMakeLists.txt new file mode 100644 index 0000000000000..74916a3d2e22f --- /dev/null +++ b/control/autoware_trajectory_follower_base/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_trajectory_follower_base) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/longitudinal_controller_base.cpp + src/lateral_controller_base.cpp +) + +ament_auto_package() diff --git a/control/autoware_trajectory_follower_base/README.md b/control/autoware_trajectory_follower_base/README.md new file mode 100644 index 0000000000000..3c79784f4b95b --- /dev/null +++ b/control/autoware_trajectory_follower_base/README.md @@ -0,0 +1,53 @@ +# Trajectory Follower + +This is the design document for the `trajectory_follower` package. + +## Purpose / Use cases + + + + +This package provides the interface of longitudinal and lateral controllers used by the node of the `autoware_trajectory_follower_node` package. +We can implement a detailed controller by deriving the longitudinal and lateral base interfaces. + +## Design + +There are lateral and longitudinal base interface classes and each algorithm inherits from this class to implement. +The interface class has the following base functions. + +- `isReady()`: Check if the control is ready to compute. +- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../autoware_trajectory_follower_node/README.md). This must be implemented by inherited algorithms. +- `sync()`: Input the result of running the other controller. + - steer angle convergence + - allow keeping stopped until steer is converged. + - velocity convergence(currently not used) + +See [the Design of Trajectory Follower Nodes](../autoware_trajectory_follower_node/README.md#Design) for how these functions work in the node. + +## Separated lateral (steering) and longitudinal (velocity) controls + +This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows. + +- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking. +- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking. + +Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below. + +### Complex requirements for longitudinal motion + +The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement. + +In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important. + +There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability. + +### Nonlinear coupling of lateral and longitudinal motion + +The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development. + +Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed. + +## Related issues + + diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp b/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/input_data.hpp similarity index 88% rename from control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp rename to control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/input_data.hpp index 665a8604214dd..679f6334b7187 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/input_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ -#define TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ +#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ +#define AUTOWARE_TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -33,4 +33,4 @@ struct InputData }; } // namespace autoware::motion::control::trajectory_follower -#endif // TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ +#endif // AUTOWARE_TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp b/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/lateral_controller_base.hpp similarity index 78% rename from control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp rename to control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/lateral_controller_base.hpp index a70c4c18fedb3..f3501336928a3 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/lateral_controller_base.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ -#define TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ +#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ +#define AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ +#include "autoware_trajectory_follower_base/input_data.hpp" +#include "autoware_trajectory_follower_base/sync_data.hpp" #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower_base/input_data.hpp" -#include "trajectory_follower_base/sync_data.hpp" #include "autoware_control_msgs/msg/lateral.hpp" @@ -44,4 +44,4 @@ class LateralControllerBase } // namespace autoware::motion::control::trajectory_follower -#endif // TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ +#endif // AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LATERAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp b/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/longitudinal_controller_base.hpp similarity index 79% rename from control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp rename to control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/longitudinal_controller_base.hpp index da5381091113f..b0f715692d079 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/longitudinal_controller_base.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ -#define TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#define AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#include "autoware_trajectory_follower_base/input_data.hpp" +#include "autoware_trajectory_follower_base/sync_data.hpp" #include "rclcpp/rclcpp.hpp" -#include "trajectory_follower_base/input_data.hpp" -#include "trajectory_follower_base/sync_data.hpp" #include "autoware_control_msgs/msg/longitudinal.hpp" @@ -46,4 +46,4 @@ class LongitudinalControllerBase } // namespace autoware::motion::control::trajectory_follower -#endif // TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ +#endif // AUTOWARE_TRAJECTORY_FOLLOWER_BASE__LONGITUDINAL_CONTROLLER_BASE_HPP_ diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp b/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/sync_data.hpp similarity index 83% rename from control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp rename to control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/sync_data.hpp index 60c91019d10c3..6034719241430 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/sync_data.hpp +++ b/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/sync_data.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ -#define TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ +#ifndef AUTOWARE_TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ +#define AUTOWARE_TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ namespace autoware::motion::control::trajectory_follower { @@ -30,4 +30,4 @@ struct LongitudinalSyncData } // namespace autoware::motion::control::trajectory_follower -#endif // TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ +#endif // AUTOWARE_TRAJECTORY_FOLLOWER_BASE__SYNC_DATA_HPP_ diff --git a/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/visibility_control.hpp b/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/visibility_control.hpp new file mode 100644 index 0000000000000..1a50424ebec43 --- /dev/null +++ b/control/autoware_trajectory_follower_base/include/autoware_trajectory_follower_base/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 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 AUTOWARE_TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) +#if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport) +#define TRAJECTORY_FOLLOWER_LOCAL +#else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport) +#define TRAJECTORY_FOLLOWER_LOCAL +#endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#elif defined(__linux__) +#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) +#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) +#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) +#else +#error "Unsupported Build Configuration" +#endif + +#endif // AUTOWARE_TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml new file mode 100644 index 0000000000000..283ab791c1ef9 --- /dev/null +++ b/control/autoware_trajectory_follower_base/package.xml @@ -0,0 +1,50 @@ + + + + autoware_trajectory_follower_base + 1.0.0 + Library for generating lateral and longitudinal controls following a trajectory + + + Takamasa Horibe + + Takayuki Murooka + + Apache 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_info_utils + autoware_vehicle_msgs + diagnostic_msgs + diagnostic_updater + eigen + geometry_msgs + interpolation + motion_utils + osqp_interface + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + eigen + + + ament_cmake + + diff --git a/control/trajectory_follower_base/src/lateral_controller_base.cpp b/control/autoware_trajectory_follower_base/src/lateral_controller_base.cpp similarity index 92% rename from control/trajectory_follower_base/src/lateral_controller_base.cpp rename to control/autoware_trajectory_follower_base/src/lateral_controller_base.cpp index 6acdbc4a8f5eb..ce5c521089328 100644 --- a/control/trajectory_follower_base/src/lateral_controller_base.cpp +++ b/control/autoware_trajectory_follower_base/src/lateral_controller_base.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower_base/lateral_controller_base.hpp" +#include "autoware_trajectory_follower_base/lateral_controller_base.hpp" namespace autoware::motion::control::trajectory_follower { diff --git a/control/trajectory_follower_base/src/longitudinal_controller_base.cpp b/control/autoware_trajectory_follower_base/src/longitudinal_controller_base.cpp similarity index 92% rename from control/trajectory_follower_base/src/longitudinal_controller_base.cpp rename to control/autoware_trajectory_follower_base/src/longitudinal_controller_base.cpp index f7779158f5791..8fac86b3883a1 100644 --- a/control/trajectory_follower_base/src/longitudinal_controller_base.cpp +++ b/control/autoware_trajectory_follower_base/src/longitudinal_controller_base.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower_base/longitudinal_controller_base.hpp" +#include "autoware_trajectory_follower_base/longitudinal_controller_base.hpp" namespace autoware::motion::control::trajectory_follower { diff --git a/control/autoware_trajectory_follower_node/CMakeLists.txt b/control/autoware_trajectory_follower_node/CMakeLists.txt new file mode 100644 index 0000000000000..cca8267d4d3be --- /dev/null +++ b/control/autoware_trajectory_follower_node/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_trajectory_follower_node) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +set(CONTROLLER_NODE controller_node) +ament_auto_add_library(${CONTROLLER_NODE} SHARED + include/autoware_trajectory_follower_node/controller_node.hpp + src/controller_node.cpp +) + +rclcpp_components_register_node(${CONTROLLER_NODE} + PLUGIN "autoware::motion::control::trajectory_follower_node::Controller" + EXECUTABLE ${CONTROLLER_NODE}_exe +) + +# simple trajectory follower +set(SIMPLE_TRAJECTORY_FOLLOWER_NODE simple_trajectory_follower) +ament_auto_add_library(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} SHARED + include/autoware_trajectory_follower_node/simple_trajectory_follower.hpp + src/simple_trajectory_follower.cpp +) + +rclcpp_components_register_node(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} + PLUGIN "simple_trajectory_follower::SimpleTrajectoryFollower" + EXECUTABLE ${SIMPLE_TRAJECTORY_FOLLOWER_NODE}_exe +) + +if(BUILD_TESTING) + set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_node) + ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} + test/trajectory_follower_test_utils.hpp + test/test_controller_node.cpp + ) + ament_target_dependencies(${TRAJECTORY_FOLLOWER_NODES_TEST} fake_test_node) + target_link_libraries( + ${TRAJECTORY_FOLLOWER_NODES_TEST} ${CONTROLLER_NODE}) + + find_package(autoware_testing REQUIRED) + # smoke test for MPC controller + add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe + PARAM_FILENAMES "lateral/mpc.param.yaml longitudinal/pid.param.yaml +trajectory_follower_node.param.yaml" + TEST_PARAM_FILENAMES "test_controller_mpc.param.yaml test_controller_pid.param.yaml +test_vehicle_info.param.yaml test_nearest_search.param.yaml" + TARGET_INFIX "mpc" + ) + # smoke test for pure pursuit controller + add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe + PARAM_FILENAMES "lateral/pure_pursuit.param.yaml longitudinal/pid.param.yaml +trajectory_follower_node.param.yaml" + TEST_PARAM_FILENAMES "test_controller_pure_pursuit.param.yaml test_controller_pid.param.yaml +test_vehicle_info.param.yaml test_nearest_search.param.yaml" + TARGET_INFIX "pure_pursuit" + ) + +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param + test + launch + config +) diff --git a/control/autoware_trajectory_follower_node/README.md b/control/autoware_trajectory_follower_node/README.md new file mode 100644 index 0000000000000..aa692c323f6d0 --- /dev/null +++ b/control/autoware_trajectory_follower_node/README.md @@ -0,0 +1,156 @@ +# Trajectory Follower Nodes + +## Purpose + +Generate control commands to follow a given Trajectory. + +## Design + +This is a node of the functionalities implemented in the controller class derived from [autoware_trajectory_follower_base](../autoware_trajectory_follower_base/README.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands. + +By default, the controller instance with the `Controller` class as follows is used. + +```plantuml +@startuml +package autoware_trajectory_follower_base { +abstract class LateralControllerBase { +longitudinal_sync_data_ + + virtual isReady(InputData) + virtual run(InputData) + sync(LongitudinalSyncData) + reset() + +} +abstract class LongitudinalControllerBase { +lateral_sync_data_ + + virtual isReady(InputData) + virtual run(InputData) + sync(LateralSyncData) + reset() + +} + +struct InputData { +trajectory +odometry +steering +accel +} +struct LongitudinalSyncData { +is_steer_converged +} +struct LateralSyncData { +} +} + +package autoware_mpc_lateral_controller { +class MPCLateralController { +isReady(InputData) override +run(InputData) override +} +} +package pure_pursuit { +class PurePursuitLateralController { +isReady(InputData) override +run(InputData) override +} +} +package pid_longitudinal_controller { +class PIDLongitudinalController { +isReady(InputData) override +run(InputData) override +} +} + +package autoware_trajectory_follower_node { +class Controller { +longitudinal_controller_ +lateral_controller_ +onTimer() +createInputData(): InputData +} +} + +MPCLateralController --|> LateralControllerBase +PurePursuitLateralController --|> LateralControllerBase +PIDLongitudinalController --|> LongitudinalControllerBase + +LateralSyncData --> LongitudinalControllerBase +LateralSyncData --> LateralControllerBase +LongitudinalSyncData --> LongitudinalControllerBase +LongitudinalSyncData --> LateralControllerBase +InputData ..> LateralControllerBase +InputData ..> LongitudinalControllerBase + +LateralControllerBase --o Controller +LongitudinalControllerBase --o Controller +InputData ..> Controller +@enduml +``` + +The process flow of `Controller` class is as follows. + +```cpp +// 1. create input data +const auto input_data = createInputData(*get_clock()); +if (!input_data) { + return; +} + +// 2. check if controllers are ready +const bool is_lat_ready = lateral_controller_->isReady(*input_data); +const bool is_lon_ready = longitudinal_controller_->isReady(*input_data); +if (!is_lat_ready || !is_lon_ready) { + return; +} + +// 3. run controllers +const auto lat_out = lateral_controller_->run(*input_data); +const auto lon_out = longitudinal_controller_->run(*input_data); + +// 4. sync with each other controllers +longitudinal_controller_->sync(lat_out.sync_data); +lateral_controller_->sync(lon_out.sync_data); + +// 5. publish control command +control_cmd_pub_->publish(out); +``` + +Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` + +- lateral controller + - `keep_steer_control_until_converged` +- longitudinal controller + - `enable_keep_stopped_until_steer_convergence` + +### Inputs / Outputs / API + +#### Inputs + +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. +- `nav_msgs/Odometry`: current odometry +- `autoware_vehicle_msgs/SteeringReport` current steering + +#### Outputs + +- `autoware_control_msgs/Control`: message containing both lateral and longitudinal commands. + +#### Parameter + +- `ctrl_period`: control commands publishing period +- `timeout_thr_sec`: duration in second after which input messages are discarded. + - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `Control` if the following two conditions are met. + 1. Both commands have been received. + 2. The last received commands are not older than defined by `timeout_thr_sec`. +- `lateral_controller_mode`: `mpc` or `pure_pursuit` + - (currently there is only `PID` for longitudinal controller) + +## Debugging + +Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. + +A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. + +In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz. diff --git a/control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml similarity index 100% rename from control/trajectory_follower_node/config/plot_juggler_trajectory_follower.xml rename to control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml diff --git a/control/trajectory_follower_node/config/simple_trajectory_follower.param.yaml b/control/autoware_trajectory_follower_node/config/simple_trajectory_follower.param.yaml similarity index 100% rename from control/trajectory_follower_node/config/simple_trajectory_follower.param.yaml rename to control/autoware_trajectory_follower_node/config/simple_trajectory_follower.param.yaml diff --git a/control/trajectory_follower_node/design/media/Controller.drawio.svg b/control/autoware_trajectory_follower_node/design/media/Controller.drawio.svg similarity index 98% rename from control/trajectory_follower_node/design/media/Controller.drawio.svg rename to control/autoware_trajectory_follower_node/design/media/Controller.drawio.svg index 1152ef2b17f59..a09758690ccdc 100644 --- a/control/trajectory_follower_node/design/media/Controller.drawio.svg +++ b/control/autoware_trajectory_follower_node/design/media/Controller.drawio.svg @@ -83,12 +83,12 @@
- trajectory_follower_node/controller_node + autoware_trajectory_follower_node/controller_node
- trajectory_follower_node/controller_node + autoware_trajectory_follower_node/controller_node @@ -140,7 +140,7 @@
trajectory_follower/
- mpc_lateral_controller + autoware_mpc_lateral_controller
diff --git a/control/trajectory_follower_node/design/simple_trajectory_follower-design.md b/control/autoware_trajectory_follower_node/design/simple_trajectory_follower-design.md similarity index 100% rename from control/trajectory_follower_node/design/simple_trajectory_follower-design.md rename to control/autoware_trajectory_follower_node/design/simple_trajectory_follower-design.md diff --git a/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/controller_node.hpp b/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/controller_node.hpp new file mode 100644 index 0000000000000..cd6ddf427d005 --- /dev/null +++ b/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/controller_node.hpp @@ -0,0 +1,145 @@ +// Copyright 2021 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 AUTOWARE_TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ +#define AUTOWARE_TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ + +#include "autoware_trajectory_follower_base/lateral_controller_base.hpp" +#include "autoware_trajectory_follower_base/longitudinal_controller_base.hpp" +#include "autoware_trajectory_follower_node/visibility_control.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#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 "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" + +#include +#include +#include + +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/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 +#include +#include +#include + +namespace autoware::motion::control +{ +using trajectory_follower::LateralOutput; +using trajectory_follower::LongitudinalOutput; +namespace trajectory_follower_node +{ + +using autoware_adapi_v1_msgs::msg::OperationModeState; +using tier4_autoware_utils::StopWatch; +using tier4_debug_msgs::msg::Float64Stamped; + +namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; + +/// \classController +/// \brief The node class used for generating longitudinal control commands (velocity/acceleration) +class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node +{ +public: + explicit Controller(const rclcpp::NodeOptions & node_options); + virtual ~Controller() {} + +private: + rclcpp::TimerBase::SharedPtr timer_control_; + double timeout_thr_sec_; + boost::optional longitudinal_output_{boost::none}; + + std::shared_ptr longitudinal_controller_; + std::shared_ptr lateral_controller_; + + // Subscribers + tier4_autoware_utils::InterProcessPollingSubscriber + sub_ref_path_{this, "~/input/reference_trajectory"}; + + tier4_autoware_utils::InterProcessPollingSubscriber sub_odometry_{ + this, "~/input/current_odometry"}; + + tier4_autoware_utils::InterProcessPollingSubscriber + sub_steering_{this, "~/input/current_steering"}; + + tier4_autoware_utils::InterProcessPollingSubscriber< + geometry_msgs::msg::AccelWithCovarianceStamped> + sub_accel_{this, "~/input/current_accel"}; + + tier4_autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ + this, "~/input/current_operation_mode"}; + + // Publishers + rclcpp::Publisher::SharedPtr control_cmd_pub_; + rclcpp::Publisher::SharedPtr pub_processing_time_lat_ms_; + rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; + rclcpp::Publisher::SharedPtr debug_marker_pub_; + + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr current_trajectory_ptr_; + nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr_; + autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr current_steering_ptr_; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_accel_ptr_; + OperationModeState::ConstSharedPtr current_operation_mode_ptr_; + + enum class LateralControllerMode { + INVALID = 0, + MPC = 1, + PURE_PURSUIT = 2, + }; + enum class LongitudinalControllerMode { + INVALID = 0, + PID = 1, + }; + + /** + * @brief compute control command, and publish periodically + */ + boost::optional createInputData(rclcpp::Clock & clock); + void callbackTimerControl(); + bool processData(rclcpp::Clock & clock); + bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out); + LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; + LongitudinalControllerMode getLongitudinalControllerMode( + const std::string & algorithm_name) const; + void publishDebugMarker( + const trajectory_follower::InputData & input_data, + const trajectory_follower::LateralOutput & lat_out) const; + + std::unique_ptr logger_configure_; + + std::unique_ptr published_time_publisher_; + + void publishProcessingTime( + const double t_ms, const rclcpp::Publisher::SharedPtr pub); + StopWatch stop_watch_; + + static constexpr double logger_throttle_interval = 5000; +}; +} // namespace trajectory_follower_node +} // namespace autoware::motion::control + +#endif // AUTOWARE_TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ diff --git a/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/simple_trajectory_follower.hpp b/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/simple_trajectory_follower.hpp new file mode 100644 index 0000000000000..094f6fb74f2ad --- /dev/null +++ b/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/simple_trajectory_follower.hpp @@ -0,0 +1,70 @@ +// Copyright 2022 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 AUTOWARE_TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#define AUTOWARE_TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ + +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace simple_trajectory_follower +{ +using autoware_control_msgs::msg::Control; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; + +class SimpleTrajectoryFollower : public rclcpp::Node +{ +public: + explicit SimpleTrajectoryFollower(const rclcpp::NodeOptions & options); + ~SimpleTrajectoryFollower() = default; + +private: + tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + this, "~/input/kinematics"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_trajectory_{ + this, "~/input/trajectory"}; + rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::TimerBase::SharedPtr timer_; + + Trajectory::ConstSharedPtr trajectory_; + Odometry::ConstSharedPtr odometry_; + TrajectoryPoint closest_traj_point_; + bool use_external_target_vel_; + double external_target_vel_; + double lateral_deviation_; + + void onTimer(); + bool processData(); + void updateClosest(); + double calcSteerCmd(); + double calcAccCmd(); +}; + +} // namespace simple_trajectory_follower + +#endif // AUTOWARE_TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ diff --git a/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/visibility_control.hpp b/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/visibility_control.hpp new file mode 100644 index 0000000000000..8f25e1a815538 --- /dev/null +++ b/control/autoware_trajectory_follower_node/include/autoware_trajectory_follower_node/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 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 AUTOWARE_TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE_TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) +#if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport) +#define TRAJECTORY_FOLLOWER_LOCAL +#else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport) +#define TRAJECTORY_FOLLOWER_LOCAL +#endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) +#elif defined(__linux__) +#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) +#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) +#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) +#else +#error "Unsupported Build Configuration" +#endif + +#endif // AUTOWARE_TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ diff --git a/control/autoware_trajectory_follower_node/launch/simple_trajectory_follower.launch.xml b/control/autoware_trajectory_follower_node/launch/simple_trajectory_follower.launch.xml new file mode 100644 index 0000000000000..521638dd7a54d --- /dev/null +++ b/control/autoware_trajectory_follower_node/launch/simple_trajectory_follower.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/control/autoware_trajectory_follower_node/package.xml b/control/autoware_trajectory_follower_node/package.xml new file mode 100644 index 0000000000000..0726b919d387e --- /dev/null +++ b/control/autoware_trajectory_follower_node/package.xml @@ -0,0 +1,50 @@ + + + + autoware_trajectory_follower_node + 1.0.0 + Nodes to follow a trajectory by generating control commands separated into lateral and longitudinal commands + + + Takamasa Horibe + + Takayuki Murooka + + Apache License 2.0 + + Takamasa Horibe + Maxime CLEMENT + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_control_msgs + autoware_mpc_lateral_controller + autoware_pid_longitudinal_controller + autoware_planning_msgs + autoware_pure_pursuit + autoware_trajectory_follower_base + autoware_vehicle_info_utils + autoware_vehicle_msgs + motion_utils + rclcpp + rclcpp_components + tier4_autoware_utils + visualization_msgs + + ros2launch + + ament_cmake_ros + ament_index_python + ament_lint_auto + autoware_lint_common + autoware_testing + fake_test_node + ros_testing + + + ament_cmake + + diff --git a/control/trajectory_follower_node/param/lateral/mpc.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml similarity index 100% rename from control/trajectory_follower_node/param/lateral/mpc.param.yaml rename to control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml diff --git a/control/trajectory_follower_node/param/lateral/pure_pursuit.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/pure_pursuit.param.yaml similarity index 100% rename from control/trajectory_follower_node/param/lateral/pure_pursuit.param.yaml rename to control/autoware_trajectory_follower_node/param/lateral/pure_pursuit.param.yaml diff --git a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml similarity index 92% rename from control/trajectory_follower_node/param/longitudinal/pid.param.yaml rename to control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index c39088753fe64..ad6217663fbae 100644 --- a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -69,8 +69,9 @@ max_jerk: 2.0 min_jerk: -5.0 - # pitch - use_trajectory_for_pitch_calculation: false + # slope compensation lpf_pitch_gain: 0.95 + slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + adaptive_trajectory_velocity_th: 1.0 max_pitch_rad: 0.1 min_pitch_rad: -0.1 diff --git a/control/trajectory_follower_node/param/trajectory_follower_node.param.yaml b/control/autoware_trajectory_follower_node/param/trajectory_follower_node.param.yaml similarity index 100% rename from control/trajectory_follower_node/param/trajectory_follower_node.param.yaml rename to control/autoware_trajectory_follower_node/param/trajectory_follower_node.param.yaml diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/autoware_trajectory_follower_node/src/controller_node.cpp similarity index 76% rename from control/trajectory_follower_node/src/controller_node.cpp rename to control/autoware_trajectory_follower_node/src/controller_node.cpp index 6fe63ca07de6f..09a8bd4e9506d 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/autoware_trajectory_follower_node/src/controller_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower_node/controller_node.hpp" +#include "autoware_trajectory_follower_node/controller_node.hpp" -#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 "autoware_mpc_lateral_controller/mpc_lateral_controller.hpp" +#include "autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp" +#include "autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" #include @@ -43,7 +43,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control break; } case LateralControllerMode::PURE_PURSUIT: { - lateral_controller_ = std::make_shared(*this); + lateral_controller_ = + std::make_shared(*this); break; } default: @@ -62,17 +63,6 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control throw std::domain_error("[LongitudinalController] invalid algorithm"); } - sub_ref_path_ = create_subscription( - "~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&Controller::onTrajectory, this, _1)); - sub_steering_ = create_subscription( - "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1)); - sub_odometry_ = create_subscription( - "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); - sub_accel_ = create_subscription( - "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1)); - sub_operation_mode_ = create_subscription( - "~/input/current_operation_mode", rclcpp::QoS{1}, - [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; }); control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); pub_processing_time_lat_ms_ = @@ -112,24 +102,32 @@ Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode return LongitudinalControllerMode::INVALID; } -void Controller::onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) +bool Controller::processData(rclcpp::Clock & clock) { - current_trajectory_ptr_ = msg; -} - -void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) -{ - current_odometry_ptr_ = msg; -} + bool is_ready = true; + + const auto & logData = [&clock, this](const std::string & data_type) { + std::string msg = "Waiting for " + data_type + " data"; + RCLCPP_INFO_THROTTLE(get_logger(), clock, logger_throttle_interval, msg.c_str()); + }; + + const auto & getData = [&logData](auto & dest, auto & sub, const std::string & data_type = "") { + const auto temp = sub.takeData(); + if (temp) { + dest = temp; + return true; + } + if (!data_type.empty()) logData(data_type); + return false; + }; -void Controller::onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg) -{ - current_steering_ptr_ = msg; -} + is_ready &= getData(current_accel_ptr_, sub_accel_, "acceleration"); + is_ready &= getData(current_steering_ptr_, sub_steering_, "steering"); + is_ready &= getData(current_trajectory_ptr_, sub_ref_path_, "trajectory"); + is_ready &= getData(current_odometry_ptr_, sub_odometry_, "odometry"); + is_ready &= getData(current_operation_mode_ptr_, sub_operation_mode_, "operation mode"); -void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - current_accel_ptr_ = msg; + return is_ready; } bool Controller::isTimeOut( @@ -152,31 +150,9 @@ bool Controller::isTimeOut( return false; } -boost::optional Controller::createInputData( - rclcpp::Clock & clock) const +boost::optional Controller::createInputData(rclcpp::Clock & clock) { - if (!current_trajectory_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for trajectory."); - return {}; - } - - if (!current_odometry_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current odometry."); - return {}; - } - - if (!current_steering_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current steering."); - return {}; - } - - if (!current_accel_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current accel."); - return {}; - } - - if (!current_operation_mode_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current operation mode."); + if (!processData(clock)) { return {}; } diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp similarity index 89% rename from control/trajectory_follower_node/src/simple_trajectory_follower.cpp rename to control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp index 7c27eaed41380..e34c67b72d30f 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/autoware_trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "trajectory_follower_node/simple_trajectory_follower.hpp" +#include "autoware_trajectory_follower_node/simple_trajectory_follower.hpp" #include #include @@ -31,11 +31,6 @@ SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & o { pub_cmd_ = create_publisher("output/control_cmd", 1); - sub_kinematics_ = create_subscription( - "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); - sub_trajectory_ = create_subscription( - "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; }); - use_external_target_vel_ = declare_parameter("use_external_target_vel"); external_target_vel_ = declare_parameter("external_target_vel"); lateral_deviation_ = declare_parameter("lateral_deviation"); @@ -47,7 +42,7 @@ SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & o void SimpleTrajectoryFollower::onTimer() { - if (!checkData()) { + if (!processData()) { RCLCPP_INFO(get_logger(), "data not ready"); return; } @@ -110,9 +105,18 @@ double SimpleTrajectoryFollower::calcAccCmd() return acc; } -bool SimpleTrajectoryFollower::checkData() +bool SimpleTrajectoryFollower::processData() { - return (trajectory_ && odometry_); + bool is_ready = true; + const auto & getData = [](auto & dest, auto & sub) { + const auto temp = sub.takeData(); + if (!temp) return false; + dest = temp; + return true; + }; + is_ready &= getData(odometry_, sub_kinematics_); + is_ready &= getData(trajectory_, sub_trajectory_); + return is_ready; } } // namespace simple_trajectory_follower diff --git a/control/trajectory_follower_node/test/test_controller_mpc.param.yaml b/control/autoware_trajectory_follower_node/test/test_controller_mpc.param.yaml similarity index 100% rename from control/trajectory_follower_node/test/test_controller_mpc.param.yaml rename to control/autoware_trajectory_follower_node/test/test_controller_mpc.param.yaml diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp similarity index 98% rename from control/trajectory_follower_node/test/test_controller_node.cpp rename to control/autoware_trajectory_follower_node/test/test_controller_node.cpp index 9bdf625226134..0ef35b222d4a1 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -13,11 +13,11 @@ // limitations under the License. #include "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware_trajectory_follower_node/controller_node.hpp" #include "fake_test_node/fake_test_node.hpp" #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "rclcpp/time.hpp" -#include "trajectory_follower_node/controller_node.hpp" #include "trajectory_follower_test_utils.hpp" #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" @@ -49,11 +49,12 @@ const rclcpp::Duration one_second(1, 0); rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_convergence = false) { // Pass default parameter file to the node - const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_trajectory_follower_node"); const auto longitudinal_share_dir = - ament_index_cpp::get_package_share_directory("pid_longitudinal_controller"); + ament_index_cpp::get_package_share_directory("autoware_pid_longitudinal_controller"); const auto lateral_share_dir = - ament_index_cpp::get_package_share_directory("mpc_lateral_controller"); + ament_index_cpp::get_package_share_directory("autoware_mpc_lateral_controller"); rclcpp::NodeOptions node_options; node_options.append_parameter_override("lateral_controller_mode", "mpc"); node_options.append_parameter_override("longitudinal_controller_mode", "pid"); diff --git a/control/trajectory_follower_node/test/test_controller_pid.param.yaml b/control/autoware_trajectory_follower_node/test/test_controller_pid.param.yaml similarity index 100% rename from control/trajectory_follower_node/test/test_controller_pid.param.yaml rename to control/autoware_trajectory_follower_node/test/test_controller_pid.param.yaml diff --git a/control/trajectory_follower_node/test/test_controller_pure_pursuit.param.yaml b/control/autoware_trajectory_follower_node/test/test_controller_pure_pursuit.param.yaml similarity index 100% rename from control/trajectory_follower_node/test/test_controller_pure_pursuit.param.yaml rename to control/autoware_trajectory_follower_node/test/test_controller_pure_pursuit.param.yaml diff --git a/control/trajectory_follower_node/test/test_nearest_search.param.yaml b/control/autoware_trajectory_follower_node/test/test_nearest_search.param.yaml similarity index 100% rename from control/trajectory_follower_node/test/test_nearest_search.param.yaml rename to control/autoware_trajectory_follower_node/test/test_nearest_search.param.yaml diff --git a/control/trajectory_follower_node/test/test_vehicle_info.param.yaml b/control/autoware_trajectory_follower_node/test/test_vehicle_info.param.yaml similarity index 100% rename from control/trajectory_follower_node/test/test_vehicle_info.param.yaml rename to control/autoware_trajectory_follower_node/test/test_vehicle_info.param.yaml diff --git a/control/trajectory_follower_node/test/trajectory_follower_test_utils.hpp b/control/autoware_trajectory_follower_node/test/trajectory_follower_test_utils.hpp similarity index 100% rename from control/trajectory_follower_node/test/trajectory_follower_test_utils.hpp rename to control/autoware_trajectory_follower_node/test/trajectory_follower_test_utils.hpp diff --git a/control/autoware_vehicle_cmd_gate/CMakeLists.txt b/control/autoware_vehicle_cmd_gate/CMakeLists.txt new file mode 100644 index 0000000000000..7233608a5d25f --- /dev/null +++ b/control/autoware_vehicle_cmd_gate/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_vehicle_cmd_gate) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(vehicle_cmd_gate_node SHARED + src/vehicle_cmd_gate.cpp + src/vehicle_cmd_filter.cpp + src/adapi_pause_interface.cpp + src/moderate_stop_interface.cpp +) + +rclcpp_components_register_node(vehicle_cmd_gate_node + PLUGIN "autoware::vehicle_cmd_gate::VehicleCmdGate" + EXECUTABLE vehicle_cmd_gate_exe +) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/IsFilterActivated.msg" + DEPENDENCIES builtin_interfaces +) + +# to use same package defined message +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(vehicle_cmd_gate_node + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(vehicle_cmd_gate_node "${cpp_typesupport_target}") +endif() + + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_vehicle_cmd_gate + test/src/test_main.cpp + test/src/test_vehicle_cmd_filter.cpp + test/src/test_filter_in_vehicle_cmd_gate_node.cpp + ) + ament_target_dependencies(test_vehicle_cmd_gate + rclcpp + tier4_control_msgs + ) + target_link_libraries(test_vehicle_cmd_gate + vehicle_cmd_gate_node + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/vehicle_cmd_gate/README.md b/control/autoware_vehicle_cmd_gate/README.md similarity index 100% rename from control/vehicle_cmd_gate/README.md rename to control/autoware_vehicle_cmd_gate/README.md diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml similarity index 100% rename from control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml rename to control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml diff --git a/control/vehicle_cmd_gate/image/filter.png b/control/autoware_vehicle_cmd_gate/image/filter.png similarity index 100% rename from control/vehicle_cmd_gate/image/filter.png rename to control/autoware_vehicle_cmd_gate/image/filter.png diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml similarity index 89% rename from control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml rename to control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index c5368276b488a..c9df4e67c2647 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -1,11 +1,11 @@ - + - + - + diff --git a/control/vehicle_cmd_gate/msg/IsFilterActivated.msg b/control/autoware_vehicle_cmd_gate/msg/IsFilterActivated.msg similarity index 100% rename from control/vehicle_cmd_gate/msg/IsFilterActivated.msg rename to control/autoware_vehicle_cmd_gate/msg/IsFilterActivated.msg diff --git a/control/autoware_vehicle_cmd_gate/package.xml b/control/autoware_vehicle_cmd_gate/package.xml new file mode 100644 index 0000000000000..f67d01a5593ae --- /dev/null +++ b/control/autoware_vehicle_cmd_gate/package.xml @@ -0,0 +1,50 @@ + + + + autoware_vehicle_cmd_gate + 0.1.0 + The vehicle_cmd_gate package + Takamasa Horibe + Tomoya Kimura + Apache License 2.0 + + Hiroki OTA + + ament_cmake + autoware_cmake + + rosidl_default_generators + + autoware_adapi_v1_msgs + autoware_control_msgs + autoware_vehicle_info_utils + autoware_vehicle_msgs + component_interface_specs + component_interface_utils + diagnostic_updater + geometry_msgs + motion_utils + rclcpp + rclcpp_components + std_srvs + tier4_api_utils + tier4_autoware_utils + tier4_control_msgs + tier4_debug_msgs + tier4_external_api_msgs + tier4_system_msgs + tier4_vehicle_msgs + visualization_msgs + + rosidl_default_runtime + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json b/control/autoware_vehicle_cmd_gate/schema/vehicle_cmd_gate.json similarity index 100% rename from control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json rename to control/autoware_vehicle_cmd_gate/schema/vehicle_cmd_gate.json diff --git a/control/vehicle_cmd_gate/script/delays_checker.py b/control/autoware_vehicle_cmd_gate/script/delays_checker.py similarity index 100% rename from control/vehicle_cmd_gate/script/delays_checker.py rename to control/autoware_vehicle_cmd_gate/script/delays_checker.py diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp similarity index 95% rename from control/vehicle_cmd_gate/src/adapi_pause_interface.cpp rename to control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp index f4cf28d337a09..f14a62fba9661 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp +++ b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp @@ -14,7 +14,7 @@ #include "adapi_pause_interface.hpp" -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { AdapiPauseInterface::AdapiPauseInterface(rclcpp::Node * node) : node_(node) @@ -65,4 +65,4 @@ void AdapiPauseInterface::on_pause( res->status.success = true; } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp similarity index 95% rename from control/vehicle_cmd_gate/src/adapi_pause_interface.hpp rename to control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp index 1d5f05e98975e..d75b544752e3f 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp +++ b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp @@ -21,7 +21,7 @@ #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { class AdapiPauseInterface @@ -55,6 +55,6 @@ class AdapiPauseInterface const SetPause::Service::Response::SharedPtr res); }; -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // ADAPI_PAUSE_INTERFACE_HPP_ diff --git a/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp b/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp new file mode 100644 index 0000000000000..c9d7e86fa23dd --- /dev/null +++ b/control/autoware_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 autoware::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 autoware::vehicle_cmd_gate + +#endif // MARKER_HELPER_HPP_ diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.cpp similarity index 95% rename from control/vehicle_cmd_gate/src/moderate_stop_interface.cpp rename to control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.cpp index 72dbdfd26a6b3..0fe90adfd0a72 100644 --- a/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp +++ b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.cpp @@ -14,7 +14,7 @@ #include "moderate_stop_interface.hpp" -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { ModerateStopInterface::ModerateStopInterface(rclcpp::Node * node) : node_(node) @@ -66,4 +66,4 @@ void ModerateStopInterface::update_stop_state() } } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp similarity index 95% rename from control/vehicle_cmd_gate/src/moderate_stop_interface.hpp rename to control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp index b643afc212f61..012e75d0c207e 100644 --- a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp +++ b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp @@ -22,7 +22,7 @@ #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { class ModerateStopInterface @@ -53,6 +53,6 @@ class ModerateStopInterface void update_stop_state(); }; -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // MODERATE_STOP_INTERFACE_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp similarity index 99% rename from control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp rename to control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index bd9955e773020..cec23b05b8191 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -17,7 +17,7 @@ #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { VehicleCmdFilter::VehicleCmdFilter() @@ -319,4 +319,4 @@ double VehicleCmdFilter::getSteerDiffLim() const return interpolateFromSpeed(param_.actual_steer_diff_lim); } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp similarity index 94% rename from control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp rename to control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index d9b8383a1de51..96663474f47a4 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -15,17 +15,17 @@ #ifndef VEHICLE_CMD_FILTER_HPP_ #define VEHICLE_CMD_FILTER_HPP_ +#include #include -#include #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { using autoware_control_msgs::msg::Control; -using vehicle_cmd_gate::msg::IsFilterActivated; +using autoware_vehicle_cmd_gate::msg::IsFilterActivated; using LimitArray = std::vector; struct VehicleCmdFilterParam @@ -98,6 +98,6 @@ class VehicleCmdFilter double getSteerRateLim() const; double getSteerDiffLim() const; }; -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // VEHICLE_CMD_FILTER_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp similarity index 88% rename from control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp rename to control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index cc9d7813083f4..e8902c9dbf5b3 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -28,7 +28,7 @@ #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { namespace @@ -87,69 +87,6 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) external_emergency_stop_heartbeat_sub_ = create_subscription( "input/external_emergency_stop_heartbeat", 1, std::bind(&VehicleCmdGate::onExternalEmergencyStopHeartbeat, this, _1)); - gate_mode_sub_ = create_subscription( - "input/gate_mode", 1, std::bind(&VehicleCmdGate::onGateMode, this, _1)); - engage_sub_ = create_subscription( - "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1)); - kinematics_sub_ = create_subscription( - "/localization/kinematic_state", 1, - [this](Odometry::SharedPtr msg) { current_kinematics_ = *msg; }); - acc_sub_ = create_subscription( - "input/acceleration", 1, [this](AccelWithCovarianceStamped::SharedPtr msg) { - current_acceleration_ = msg->accel.accel.linear.x; - }); - steer_sub_ = create_subscription( - "input/steering", 1, - [this](SteeringReport::SharedPtr msg) { current_steer_ = msg->steering_tire_angle; }); - operation_mode_sub_ = create_subscription( - "input/operation_mode", rclcpp::QoS(1).transient_local(), - [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; }); - mrm_state_sub_ = create_subscription( - "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); - - // Subscriber for auto - auto_control_cmd_sub_ = create_subscription( - "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1)); - - auto_turn_indicator_cmd_sub_ = create_subscription( - "input/auto/turn_indicators_cmd", 1, - [this](TurnIndicatorsCommand::ConstSharedPtr msg) { auto_commands_.turn_indicator = *msg; }); - - auto_hazard_light_cmd_sub_ = create_subscription( - "input/auto/hazard_lights_cmd", 1, - [this](HazardLightsCommand::ConstSharedPtr msg) { auto_commands_.hazard_light = *msg; }); - - auto_gear_cmd_sub_ = create_subscription( - "input/auto/gear_cmd", 1, - [this](GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; }); - - // Subscriber for external - remote_control_cmd_sub_ = create_subscription( - "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1)); - - remote_turn_indicator_cmd_sub_ = create_subscription( - "input/external/turn_indicators_cmd", 1, - [this](TurnIndicatorsCommand::ConstSharedPtr msg) { remote_commands_.turn_indicator = *msg; }); - - remote_hazard_light_cmd_sub_ = create_subscription( - "input/external/hazard_lights_cmd", 1, - [this](HazardLightsCommand::ConstSharedPtr msg) { remote_commands_.hazard_light = *msg; }); - - remote_gear_cmd_sub_ = create_subscription( - "input/external/gear_cmd", 1, - [this](GearCommand::ConstSharedPtr msg) { remote_commands_.gear = *msg; }); - - // Subscriber for emergency - emergency_control_cmd_sub_ = create_subscription( - "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1)); - - emergency_hazard_light_cmd_sub_ = create_subscription( - "input/emergency/hazard_lights_cmd", 1, - [this](HazardLightsCommand::ConstSharedPtr msg) { emergency_commands_.hazard_light = *msg; }); - - emergency_gear_cmd_sub_ = create_subscription( - "input/emergency/gear_cmd", 1, - [this](GearCommand::ConstSharedPtr msg) { emergency_commands_.gear = *msg; }); // Parameter use_emergency_handling_ = declare_parameter("use_emergency_handling"); @@ -170,7 +107,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) declare_parameter("filter_activated_velocity_threshold"); // Vehicle Parameter - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); { VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; @@ -276,7 +213,7 @@ rcl_interfaces::msg::SetParametersResult VehicleCmdGate::onParameter( parameters, "filter_activated_velocity_threshold", filter_activated_velocity_threshold_); // Vehicle Parameter - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); { VehicleCmdFilterParam p = filter_.getParam(); p.wheel_base = vehicle_info.wheel_base_m; @@ -354,9 +291,10 @@ bool VehicleCmdGate::isDataReady() } // for auto -void VehicleCmdGate::onAutoCtrlCmd(Control::ConstSharedPtr msg) +void VehicleCmdGate::onAutoCtrlCmd() { - auto_commands_.control = *msg; + const auto msg = auto_control_cmd_sub_.takeData(); + if (msg) auto_commands_.control = *msg; if (current_gate_mode_.data == GateMode::AUTO) { publishControlCommands(auto_commands_); @@ -364,9 +302,10 @@ void VehicleCmdGate::onAutoCtrlCmd(Control::ConstSharedPtr msg) } // for remote -void VehicleCmdGate::onRemoteCtrlCmd(Control::ConstSharedPtr msg) +void VehicleCmdGate::onRemoteCtrlCmd() { - remote_commands_.control = *msg; + const auto msg = remote_control_cmd_sub_.takeData(); + if (msg) remote_commands_.control = *msg; if (current_gate_mode_.data == GateMode::EXTERNAL) { publishControlCommands(remote_commands_); @@ -374,9 +313,10 @@ void VehicleCmdGate::onRemoteCtrlCmd(Control::ConstSharedPtr msg) } // for emergency -void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg) +void VehicleCmdGate::onEmergencyCtrlCmd() { - emergency_commands_.control = *msg; + const auto msg = emergency_control_cmd_sub_.takeData(); + if (msg) emergency_commands_.control = *msg; if (use_emergency_handling_ && is_system_emergency_) { publishControlCommands(emergency_commands_); @@ -385,6 +325,61 @@ void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg) void VehicleCmdGate::onTimer() { + onGateMode(); + + const auto msg_kinematics = kinematics_sub_.takeData(); + if (msg_kinematics) current_kinematics_ = *msg_kinematics; + + const auto msg_acceleration = acc_sub_.takeData(); + if (msg_acceleration) current_acceleration_ = msg_acceleration->accel.accel.linear.x; + + const auto msg_steer = steer_sub_.takeData(); + if (msg_steer) current_steer_ = msg_steer->steering_tire_angle; + + const auto msg_operation_mode = operation_mode_sub_.takeData(); + if (msg_operation_mode) current_operation_mode_ = *msg_operation_mode; + + onMrmState(); + + // Subscriber for auto + onAutoCtrlCmd(); + + const auto msg_auto_command_turn_indicator = auto_turn_indicator_cmd_sub_.takeData(); + if (msg_auto_command_turn_indicator) + auto_commands_.turn_indicator = *msg_auto_command_turn_indicator; + + const auto msg_auto_command_hazard_light = auto_hazard_light_cmd_sub_.takeData(); + if (msg_auto_command_hazard_light) auto_commands_.hazard_light = *msg_auto_command_hazard_light; + + const auto msg_auto_command_gear = auto_gear_cmd_sub_.takeData(); + if (msg_auto_command_gear) auto_commands_.gear = *msg_auto_command_gear; + + // Subscribe for external + onRemoteCtrlCmd(); + + const auto msg_remote_command_turn_indicator = remote_turn_indicator_cmd_sub_.takeData(); + if (msg_remote_command_turn_indicator) + remote_commands_.turn_indicator = *msg_remote_command_turn_indicator; + + const auto msg_remote_command_hazard_light = remote_hazard_light_cmd_sub_.takeData(); + if (msg_remote_command_hazard_light) + remote_commands_.hazard_light = *msg_remote_command_hazard_light; + + const auto msg_remote_command_gear = remote_gear_cmd_sub_.takeData(); + if (msg_remote_command_gear) remote_commands_.gear = *msg_remote_command_gear; + + // Subscribe for emergency + onEmergencyCtrlCmd(); + + const auto msg_emergency_command_hazard_light = emergency_hazard_light_cmd_sub_.takeData(); + if (msg_emergency_command_hazard_light) + emergency_commands_.hazard_light = *msg_emergency_command_hazard_light; + + const auto msg_emergency_command_gear = emergency_gear_cmd_sub_.takeData(); + if (msg_emergency_command_gear) emergency_commands_.gear = *msg_emergency_command_gear; + + onEngage(); + updater_.force_update(); if (!isDataReady()) { @@ -710,10 +705,12 @@ void VehicleCmdGate::onExternalEmergencyStopHeartbeat( external_emergency_stop_heartbeat_received_time_ = std::make_shared(this->now()); } -void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg) +void VehicleCmdGate::onGateMode() { + const auto msg = gate_mode_sub_.takeData(); + if (!msg) return; + const auto prev_gate_mode = current_gate_mode_; - current_gate_mode_ = *msg; if (current_gate_mode_.data != prev_gate_mode.data) { RCLCPP_INFO( @@ -722,9 +719,10 @@ void VehicleCmdGate::onGateMode(GateMode::ConstSharedPtr msg) } } -void VehicleCmdGate::onEngage(EngageMsg::ConstSharedPtr msg) +void VehicleCmdGate::onEngage() { - is_engaged_ = msg->engage; + const auto msg = engage_sub_.takeData(); + if (msg) is_engaged_ = msg->engage; } void VehicleCmdGate::onEngageService( @@ -734,8 +732,11 @@ void VehicleCmdGate::onEngageService( response->status = tier4_api_utils::response_success(); } -void VehicleCmdGate::onMrmState(MrmState::ConstSharedPtr msg) +void VehicleCmdGate::onMrmState() { + const auto msg = mrm_state_sub_.takeData(); + if (!msg) return; + is_system_emergency_ = (msg->state == MrmState::MRM_OPERATING || msg->state == MrmState::MRM_SUCCEEDED || msg->state == MrmState::MRM_FAILED) && @@ -906,7 +907,7 @@ void VehicleCmdGate::publishMarkers(const IsFilterActivated & filter_activated) filter_activated_flag_pub_->publish(filter_activated_flag); filter_activated_marker_raw_pub_->publish(createMarkerArray(filter_activated)); } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #include -RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_cmd_gate::VehicleCmdGate) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::vehicle_cmd_gate::VehicleCmdGate) diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp similarity index 75% rename from control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp rename to control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index f45280d9d2e48..5ca8db5497e59 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -20,12 +20,13 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_cmd_filter.hpp" +#include +#include #include #include #include +#include #include -#include -#include #include #include @@ -51,13 +52,14 @@ #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_control_msgs::msg::Control; using autoware_control_msgs::msg::Longitudinal; +using autoware_vehicle_cmd_gate::msg::IsFilterActivated; using autoware_vehicle_msgs::msg::GearCommand; using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::SteeringReport; @@ -71,7 +73,6 @@ using tier4_external_api_msgs::msg::Heartbeat; 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; @@ -118,16 +119,22 @@ class VehicleCmdGate : public rclcpp::Node const std::vector & parameters); // Subscription rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; - rclcpp::Subscription::SharedPtr gate_mode_sub_; - rclcpp::Subscription::SharedPtr operation_mode_sub_; - rclcpp::Subscription::SharedPtr mrm_state_sub_; - rclcpp::Subscription::SharedPtr kinematics_sub_; // for filter - rclcpp::Subscription::SharedPtr acc_sub_; // for filter - rclcpp::Subscription::SharedPtr steer_sub_; // for filter - - void onGateMode(GateMode::ConstSharedPtr msg); + tier4_autoware_utils::InterProcessPollingSubscriber gate_mode_sub_{ + this, "input/gate_mode"}; + tier4_autoware_utils::InterProcessPollingSubscriber operation_mode_sub_{ + this, "input/operation_mode", rclcpp::QoS(1).transient_local()}; + tier4_autoware_utils::InterProcessPollingSubscriber mrm_state_sub_{ + this, "input/mrm_state"}; + tier4_autoware_utils::InterProcessPollingSubscriber kinematics_sub_{ + this, "/localization/kinematic_state"}; // for filter + tier4_autoware_utils::InterProcessPollingSubscriber acc_sub_{ + this, "input/acceleration"}; // for filter + tier4_autoware_utils::InterProcessPollingSubscriber steer_sub_{ + this, "input/steering"}; // for filter + + void onGateMode(); void onExternalEmergencyStopHeartbeat(Heartbeat::ConstSharedPtr msg); - void onMrmState(MrmState::ConstSharedPtr msg); + void onMrmState(); bool is_engaged_; bool is_system_emergency_ = false; @@ -153,26 +160,37 @@ class VehicleCmdGate : public rclcpp::Node // Subscriber for auto Commands auto_commands_; - rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; - rclcpp::Subscription::SharedPtr auto_turn_indicator_cmd_sub_; - rclcpp::Subscription::SharedPtr auto_hazard_light_cmd_sub_; - rclcpp::Subscription::SharedPtr auto_gear_cmd_sub_; - void onAutoCtrlCmd(Control::ConstSharedPtr msg); + tier4_autoware_utils::InterProcessPollingSubscriber auto_control_cmd_sub_{ + this, "input/auto/control_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber + auto_turn_indicator_cmd_sub_{this, "input/auto/turn_indicators_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber + auto_hazard_light_cmd_sub_{this, "input/auto/hazard_lights_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber auto_gear_cmd_sub_{ + this, "input/auto/gear_cmd"}; + void onAutoCtrlCmd(); // Subscription for external Commands remote_commands_; - rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; - rclcpp::Subscription::SharedPtr remote_turn_indicator_cmd_sub_; - rclcpp::Subscription::SharedPtr remote_hazard_light_cmd_sub_; - rclcpp::Subscription::SharedPtr remote_gear_cmd_sub_; - void onRemoteCtrlCmd(Control::ConstSharedPtr msg); + tier4_autoware_utils::InterProcessPollingSubscriber remote_control_cmd_sub_{ + this, "input/external/control_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber + remote_turn_indicator_cmd_sub_{this, "input/external/turn_indicators_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber + remote_hazard_light_cmd_sub_{this, "input/external/hazard_lights_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber remote_gear_cmd_sub_{ + this, "input/external/gear_cmd"}; + void onRemoteCtrlCmd(); // Subscription for emergency Commands emergency_commands_; - rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; - rclcpp::Subscription::SharedPtr emergency_hazard_light_cmd_sub_; - rclcpp::Subscription::SharedPtr emergency_gear_cmd_sub_; - void onEmergencyCtrlCmd(Control::ConstSharedPtr msg); + tier4_autoware_utils::InterProcessPollingSubscriber emergency_control_cmd_sub_{ + this, "input/emergency/control_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber + emergency_hazard_light_cmd_sub_{this, "input/emergency/hazard_lights_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber emergency_gear_cmd_sub_{ + this, "input/emergency/gear_cmd"}; + void onEmergencyCtrlCmd(); // Parameter bool use_emergency_handling_; @@ -198,10 +216,10 @@ class VehicleCmdGate : public rclcpp::Node const SetEmergency::Response::SharedPtr response); // TODO(Takagi, Isamu): deprecated - rclcpp::Subscription::SharedPtr engage_sub_; + tier4_autoware_utils::InterProcessPollingSubscriber engage_sub_{this, "input/engage"}; rclcpp::Service::SharedPtr srv_external_emergency_stop_; rclcpp::Service::SharedPtr srv_clear_external_emergency_stop_; - void onEngage(EngageMsg::ConstSharedPtr msg); + void onEngage(); bool onSetExternalEmergencyStopService( const std::shared_ptr req_header, const Trigger::Request::SharedPtr req, const Trigger::Response::SharedPtr res); @@ -257,5 +275,5 @@ class VehicleCmdGate : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // VEHICLE_CMD_GATE_HPP_ diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp similarity index 98% rename from control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp rename to control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index d51db90c8a260..46130fc8f9941 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -54,7 +54,7 @@ const std::vector lat_jerk_lim = {1.7, 1.3, 0.9, 0.6}; const std::vector actual_steer_diff_lim = {0.5, 0.4, 0.2, 0.1}; const double wheelbase = 2.89; -using vehicle_cmd_gate::VehicleCmdGate; +using autoware::vehicle_cmd_gate::VehicleCmdGate; using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; @@ -369,9 +369,9 @@ std::shared_ptr generateNode() auto node_options = rclcpp::NodeOptions{}; const auto vehicle_cmd_gate_dir = - ament_index_cpp::get_package_share_directory("vehicle_cmd_gate"); + ament_index_cpp::get_package_share_directory("autoware_vehicle_cmd_gate"); const auto vehicle_info_util_dir = - ament_index_cpp::get_package_share_directory("vehicle_info_util"); + ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils"); node_options.arguments( {"--ros-args", "--params-file", vehicle_cmd_gate_dir + "/config/vehicle_cmd_gate.param.yaml", diff --git a/control/vehicle_cmd_gate/test/src/test_main.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_main.cpp similarity index 100% rename from control/vehicle_cmd_gate/test/src/test_main.cpp rename to control/autoware_vehicle_cmd_gate/test/src/test_main.cpp diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp similarity index 98% rename from control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp rename to control/autoware_vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 5fbab1c5cfb4f..528253671b38a 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/autoware_vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -24,17 +24,17 @@ #define ASSERT_LT_NEAR(x, y) ASSERT_LT(x, y + THRESHOLD) #define ASSERT_GT_NEAR(x, y) ASSERT_GT(x, y - THRESHOLD) +using autoware::vehicle_cmd_gate::LimitArray; using autoware_control_msgs::msg::Control; -using vehicle_cmd_gate::LimitArray; constexpr double NOMINAL_INTERVAL = 1.0; void setFilterParams( - vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, + autoware::vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, 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; + autoware::vehicle_cmd_gate::VehicleCmdFilterParam p; p.vel_lim = v; p.wheel_base = wheelbase; p.reference_speed_points = speed_points; @@ -105,7 +105,7 @@ void test_1d_limit( const double WHEELBASE = 3.0; const double DT = 0.1; // [s] - vehicle_cmd_gate::VehicleCmdFilter filter; + autoware::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}, {STEER_LIM}, @@ -275,9 +275,9 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) { constexpr double WHEELBASE = 2.8; - vehicle_cmd_gate::VehicleCmdFilter filter; + autoware::vehicle_cmd_gate::VehicleCmdFilter filter; - vehicle_cmd_gate::VehicleCmdFilterParam p; + autoware::vehicle_cmd_gate::VehicleCmdFilterParam p; p.wheel_base = WHEELBASE; p.vel_lim = 20.0; p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; diff --git a/control/control_performance_analysis/launch/control_performance_analysis.launch.xml b/control/control_performance_analysis/launch/control_performance_analysis.launch.xml index be2b30bdf1bf9..f2997f0e10b83 100644 --- a/control/control_performance_analysis/launch/control_performance_analysis.launch.xml +++ b/control/control_performance_analysis/launch/control_performance_analysis.launch.xml @@ -8,7 +8,7 @@ - + diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index a26f4164a177b..b18fb8c98ccdc 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -26,6 +26,7 @@ autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs libboost-dev @@ -41,7 +42,6 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs - vehicle_info_util builtin_interfaces global_parameter_loader rosidl_default_runtime 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 693194e8b70a6..47ce3ce07e83c 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -17,7 +17,7 @@ #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" -#include +#include #include #include @@ -32,7 +32,7 @@ using control_performance_analysis::msg::ErrorStamped; namespace control_performance_analysis { -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfoUtils; ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( const rclcpp::NodeOptions & node_options) @@ -41,7 +41,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( using std::placeholders::_1; // Implement Reading Global and Local Variables. - const auto & vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + const auto & vehicle_info = VehicleInfoUtils(*this).getVehicleInfo(); param_.wheelbase_ = vehicle_info.wheel_base_m; // Node Parameters. diff --git a/control/control_validator/CMakeLists.txt b/control/control_validator/CMakeLists.txt deleted file mode 100644 index fab942c4dc001..0000000000000 --- a/control/control_validator/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(control_validator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(control_validator_helpers SHARED - src/utils.cpp - src/debug_marker.cpp -) - -# control validator -ament_auto_add_library(control_validator_component SHARED - include/control_validator/control_validator.hpp - src/control_validator.cpp -) -target_link_libraries(control_validator_component control_validator_helpers) -rclcpp_components_register_node(control_validator_component - PLUGIN "control_validator::ControlValidator" - EXECUTABLE control_validator_node -) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "msg/ControlValidatorStatus.msg" - DEPENDENCIES builtin_interfaces -) - -# to use a message defined in the same package -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(control_validator_component - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(control_validator_component "${cpp_typesupport_target}") -endif() - -# if(BUILD_TESTING) -# endif() - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp deleted file mode 100644 index e048ef03bf11a..0000000000000 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ /dev/null @@ -1,99 +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 CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ -#define CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ - -#include "control_validator/debug_marker.hpp" -#include "control_validator/msg/control_validator_status.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include -#include - -#include -#include -#include - -#include -#include - -namespace control_validator -{ -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; -using control_validator::msg::ControlValidatorStatus; -using diagnostic_updater::DiagnosticStatusWrapper; -using diagnostic_updater::Updater; -using nav_msgs::msg::Odometry; - -struct ValidationParams -{ - double max_distance_deviation_threshold; -}; - -class ControlValidator : public rclcpp::Node -{ -public: - explicit ControlValidator(const rclcpp::NodeOptions & options); - - void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); - void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); - - bool checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory); - -private: - void setupDiag(); - - void setupParameters(); - - bool isDataReady(); - - void validate(const Trajectory & trajectory); - - void publishPredictedTrajectory(); - void publishDebugInfo(); - void displayStatus(); - - void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - - rclcpp::Subscription::SharedPtr sub_kinematics_; - rclcpp::Subscription::SharedPtr sub_reference_traj_; - rclcpp::Subscription::SharedPtr sub_predicted_traj_; - rclcpp::Publisher::SharedPtr pub_status_; - rclcpp::Publisher::SharedPtr pub_markers_; - - // system parameters - int diag_error_count_threshold_ = 0; - bool display_on_terminal_ = true; - - Updater diag_updater_{this}; - - ControlValidatorStatus validation_status_; - ValidationParams validation_params_; // for thresholds - - vehicle_info_util::VehicleInfo vehicle_info_; - - bool isAllValid(const ControlValidatorStatus & status); - - Trajectory::ConstSharedPtr current_reference_trajectory_; - Trajectory::ConstSharedPtr current_predicted_trajectory_; - - Odometry::ConstSharedPtr current_kinematics_; - - std::shared_ptr debug_pose_publisher_; -}; -} // namespace control_validator - -#endif // CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ diff --git a/control/control_validator/include/control_validator/debug_marker.hpp b/control/control_validator/include/control_validator/debug_marker.hpp deleted file mode 100644 index 2d3a209cbd7da..0000000000000 --- a/control/control_validator/include/control_validator/debug_marker.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 CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ -#define CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ - -#include - -#include -#include -#include - -#include -#include -#include -#include - -class ControlValidatorDebugMarkerPublisher -{ -public: - explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node); - - void pushPoseMarker( - const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0); - void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); - void pushVirtualWall(const geometry_msgs::msg::Pose & pose); - void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); - void publish(); - - void clearMarkers(); - -private: - rclcpp::Node * node_; - visualization_msgs::msg::MarkerArray marker_array_; - visualization_msgs::msg::MarkerArray marker_array_virtual_wall_; - rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr virtual_wall_pub_; - std::map marker_id_; - - int getMarkerId(const std::string & ns) - { - if (marker_id_.count(ns) == 0) { - marker_id_[ns] = 0.0; - } - return marker_id_[ns]++; - } -}; - -#endif // CONTROL_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/control/control_validator/include/control_validator/utils.hpp b/control/control_validator/include/control_validator/utils.hpp deleted file mode 100644 index edf97aaf5f510..0000000000000 --- a/control/control_validator/include/control_validator/utils.hpp +++ /dev/null @@ -1,55 +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 CONTROL_VALIDATOR__UTILS_HPP_ -#define CONTROL_VALIDATOR__UTILS_HPP_ - -#include -#include - -#include - -#include -#include -#include - -namespace control_validator -{ -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; -using geometry_msgs::msg::Pose; -using motion_utils::convertToTrajectory; -using motion_utils::convertToTrajectoryPointArray; -using TrajectoryPoints = std::vector; - -void shiftPose(Pose & pose, double longitudinal); - -void insertPointInPredictedTrajectory( - TrajectoryPoints & modified_trajectory, const geometry_msgs::msg::Pose & reference_pose, - const TrajectoryPoints & predicted_trajectory); - -TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory); - -bool removeFrontTrajectoryPoint( - const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, - const TrajectoryPoints & predicted_trajectory_points); - -Trajectory alignTrajectoryWithReferenceTrajectory( - const Trajectory & trajectory, const Trajectory & predicted_trajectory); - -double calcMaxLateralDistance( - const Trajectory & trajectory, const Trajectory & predicted_trajectory); -} // namespace control_validator - -#endif // CONTROL_VALIDATOR__UTILS_HPP_ diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml deleted file mode 100644 index 2f4400d6ebc57..0000000000000 --- a/control/control_validator/package.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - control_validator - 0.1.0 - ros node for control_validator - Kyoichi Sugahara - Takamasa Horibe - Makoto Kurihara - Mamoru Sobue - Takayuki Murooka - - Apache License 2.0 - - Kyoichi Sugahara - Takamasa Horibe - Makoto Kurihara - - ament_cmake_auto - autoware_cmake - rosidl_default_generators - - autoware_planning_msgs - diagnostic_updater - geometry_msgs - motion_utils - nav_msgs - rclcpp - rclcpp_components - tier4_autoware_utils - vehicle_info_util - visualization_msgs - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - rosidl_default_runtime - rosidl_interface_packages - - - ament_cmake - - diff --git a/control/control_validator/src/debug_marker.cpp b/control/control_validator/src/debug_marker.cpp deleted file mode 100644 index 2dbdc558b305b..0000000000000 --- a/control/control_validator/src/debug_marker.cpp +++ /dev/null @@ -1,101 +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 "control_validator/debug_marker.hpp" - -#include -#include - -#include -#include -#include - -using visualization_msgs::msg::Marker; - -ControlValidatorDebugMarkerPublisher::ControlValidatorDebugMarkerPublisher(rclcpp::Node * node) -: node_(node) -{ - debug_viz_pub_ = - node_->create_publisher("~/debug/marker", 1); - - virtual_wall_pub_ = - node_->create_publisher("~/virtual_wall", 1); -} - -void ControlValidatorDebugMarkerPublisher::clearMarkers() -{ - marker_array_.markers.clear(); - marker_array_virtual_wall_.markers.clear(); -} - -void ControlValidatorDebugMarkerPublisher::pushPoseMarker( - const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) -{ - pushPoseMarker(p.pose, ns, id); -} - -void ControlValidatorDebugMarkerPublisher::pushPoseMarker( - const geometry_msgs::msg::Pose & pose, const std::string & ns, int id) -{ - using tier4_autoware_utils::createMarkerColor; - - // append arrow marker - std_msgs::msg::ColorRGBA color; - if (id == 0) // Red - { - color = createMarkerColor(1.0, 0.0, 0.0, 0.999); - } - if (id == 1) // Green - { - color = createMarkerColor(0.0, 1.0, 0.0, 0.999); - } - if (id == 2) // Blue - { - color = createMarkerColor(0.0, 0.0, 1.0, 0.999); - } - Marker marker = tier4_autoware_utils::createDefaultMarker( - "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::ARROW, - tier4_autoware_utils::createMarkerScale(0.2, 0.1, 0.3), color); - marker.lifetime = rclcpp::Duration::from_seconds(0.2); - marker.pose = pose; - - marker_array_.markers.push_back(marker); -} - -void ControlValidatorDebugMarkerPublisher::pushWarningMsg( - const geometry_msgs::msg::Pose & pose, const std::string & msg) -{ - visualization_msgs::msg::Marker marker = tier4_autoware_utils::createDefaultMarker( - "map", node_->get_clock()->now(), "warning_msg", 0, Marker::TEXT_VIEW_FACING, - tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), - tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.999)); - marker.lifetime = rclcpp::Duration::from_seconds(0.2); - marker.pose = pose; - marker.text = msg; - marker_array_virtual_wall_.markers.push_back(marker); -} - -void ControlValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs::msg::Pose & pose) -{ - const auto now = node_->get_clock()->now(); - const auto stop_wall_marker = - motion_utils::createStopVirtualWallMarker(pose, "control_validator", now, 0); - tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); -} - -void ControlValidatorDebugMarkerPublisher::publish() -{ - debug_viz_pub_->publish(marker_array_); - virtual_wall_pub_->publish(marker_array_virtual_wall_); -} diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp deleted file mode 100644 index fab8b9f6f888b..0000000000000 --- a/control/control_validator/src/utils.cpp +++ /dev/null @@ -1,165 +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 "control_validator/utils.hpp" - -#include -#include -#include - -#include -#include -#include -#include - -namespace control_validator -{ - -void shiftPose(Pose & pose, double longitudinal) -{ - const auto yaw = tf2::getYaw(pose.orientation); - pose.position.x += std::cos(yaw) * longitudinal; - pose.position.y += std::sin(yaw) * longitudinal; -} - -void insertPointInPredictedTrajectory( - TrajectoryPoints & modified_trajectory, const Pose & reference_pose, - const TrajectoryPoints & predicted_trajectory) -{ - const auto point_to_interpolate = - motion_utils::calcInterpolatedPoint(convertToTrajectory(predicted_trajectory), reference_pose); - modified_trajectory.insert(modified_trajectory.begin(), point_to_interpolate); -} - -TrajectoryPoints reverseTrajectoryPoints(const TrajectoryPoints & trajectory_points) -{ - 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)); - return reversed_trajectory_points; -} - -bool removeFrontTrajectoryPoint( - const TrajectoryPoints & trajectory_points, TrajectoryPoints & modified_trajectory_points, - const TrajectoryPoints & predicted_trajectory_points) -{ - bool predicted_trajectory_point_removed = false; - for (const auto & point : predicted_trajectory_points) { - if ( - motion_utils::calcLongitudinalOffsetToSegment(trajectory_points, 0, point.pose.position) < - 0.0) { - modified_trajectory_points.erase(modified_trajectory_points.begin()); - - predicted_trajectory_point_removed = true; - } else { - break; - } - } - - return predicted_trajectory_point_removed; -} - -Trajectory alignTrajectoryWithReferenceTrajectory( - const Trajectory & trajectory, const Trajectory & predicted_trajectory) -{ - const auto last_seg_length = motion_utils::calcSignedArcLength( - trajectory.points, trajectory.points.size() - 2, trajectory.points.size() - 1); - - // If no overlapping between trajectory and predicted_trajectory, return empty trajectory - // predicted_trajectory: p1------------------pN - // trajectory: t1------------------tN - // OR - // predicted_trajectory: p1------------------pN - // trajectory: t1------------------tN - const bool & is_p_n_before_t1 = - motion_utils::calcLongitudinalOffsetToSegment( - trajectory.points, 0, predicted_trajectory.points.back().pose.position) < 0.0; - const bool & is_p1_behind_t_n = motion_utils::calcLongitudinalOffsetToSegment( - trajectory.points, trajectory.points.size() - 2, - predicted_trajectory.points.front().pose.position) - - last_seg_length > - 0.0; - const bool is_no_overlapping = (is_p_n_before_t1 || is_p1_behind_t_n); - - if (is_no_overlapping) { - return Trajectory(); - } - - auto modified_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); - auto predicted_trajectory_points = convertToTrajectoryPointArray(predicted_trajectory); - auto trajectory_points = convertToTrajectoryPointArray(trajectory); - - // If first point of predicted_trajectory is in front of start of trajectory, erase points which - // are in front of trajectory start point and insert pNew along the predicted_trajectory - // predicted_trajectory: p1-----p2-----p3----//------pN - // trajectory: t1--------//------tN - // ↓ - // predicted_trajectory: pNew--p3----//------pN - // trajectory: t1--------//------tN - auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( - trajectory_points, modified_trajectory_points, predicted_trajectory_points); - - if (predicted_trajectory_point_removed) { - insertPointInPredictedTrajectory( - modified_trajectory_points, trajectory_points.front().pose, predicted_trajectory_points); - } - - // If last point of predicted_trajectory is behind of end of trajectory, erase points which are - // behind trajectory last point and insert pNew along the predicted_trajectory - // predicted_trajectory: p1-----//------pN-2-----pN-1-----pN - // trajectory: t1-----//-----tN-1--tN - // ↓ - // predicted_trajectory: p1-----//------pN-2-pNew - // trajectory: t1-----//-----tN-1--tN - - auto reversed_predicted_trajectory_points = reverseTrajectoryPoints(predicted_trajectory_points); - auto reversed_trajectory_points = reverseTrajectoryPoints(trajectory_points); - auto reversed_modified_trajectory_points = reverseTrajectoryPoints(modified_trajectory_points); - - auto reversed_predicted_trajectory_point_removed = removeFrontTrajectoryPoint( - reversed_trajectory_points, reversed_modified_trajectory_points, - reversed_predicted_trajectory_points); - - if (reversed_predicted_trajectory_point_removed) { - insertPointInPredictedTrajectory( - reversed_modified_trajectory_points, reversed_trajectory_points.front().pose, - reversed_predicted_trajectory_points); - } - - return convertToTrajectory(reverseTrajectoryPoints(reversed_modified_trajectory_points)); -} - -double calcMaxLateralDistance( - const Trajectory & reference_trajectory, const Trajectory & predicted_trajectory) -{ - const auto alined_predicted_trajectory = - alignTrajectoryWithReferenceTrajectory(reference_trajectory, predicted_trajectory); - double max_dist = 0; - for (const auto & point : alined_predicted_trajectory.points) { - const auto p0 = tier4_autoware_utils::getPoint(point); - // find nearest segment - const size_t nearest_segment_idx = - motion_utils::findNearestSegmentIndex(reference_trajectory.points, p0); - 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; - } - } - return max_dist; -} - -} // namespace control_validator diff --git a/control/external_cmd_selector/CMakeLists.txt b/control/external_cmd_selector/CMakeLists.txt deleted file mode 100644 index 5c4860b7441f3..0000000000000 --- a/control/external_cmd_selector/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(external_cmd_selector) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(external_cmd_selector_node SHARED - src/external_cmd_selector/external_cmd_selector_node.cpp -) - -rclcpp_components_register_node(external_cmd_selector_node - PLUGIN "ExternalCmdSelector" - EXECUTABLE external_cmd_selector -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/control/external_cmd_selector/README.md b/control/external_cmd_selector/README.md deleted file mode 100644 index ca0f4f0954dbf..0000000000000 --- a/control/external_cmd_selector/README.md +++ /dev/null @@ -1,34 +0,0 @@ -# external_cmd_selector - -## Purpose - -`external_cmd_selector` is the package to publish `external_control_cmd`, `gear_cmd`, `hazard_lights_cmd`, `heartbeat` and `turn_indicators_cmd`, according to the current mode, which is `remote` or `local`. - -The current mode is set via service, `remote` is remotely operated, `local` is to use the values calculated by Autoware. - -## Input / Output - -### Input topics - -| Name | Type | Description | -| ---------------------------------------------- | ---- | ------------------------------------------------------- | -| `/api/external/set/command/local/control` | TBD | Local. Calculated control value. | -| `/api/external/set/command/local/heartbeat` | TBD | Local. Heartbeat. | -| `/api/external/set/command/local/shift` | TBD | Local. Gear shift like drive, rear and etc. | -| `/api/external/set/command/local/turn_signal` | TBD | Local. Turn signal like left turn, right turn and etc. | -| `/api/external/set/command/remote/control` | TBD | Remote. Calculated control value. | -| `/api/external/set/command/remote/heartbeat` | TBD | Remote. Heartbeat. | -| `/api/external/set/command/remote/shift` | TBD | Remote. Gear shift like drive, rear and etc. | -| `/api/external/set/command/remote/turn_signal` | TBD | Remote. Turn signal like left turn, right turn and etc. | - -### Output topics - -| Name | Type | Description | -| ------------------------------------------------------ | ------------------------------------------------- | ----------------------------------------------- | -| `/control/external_cmd_selector/current_selector_mode` | TBD | Current selected mode, remote or local. | -| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Check if node is active or not. | -| `/external/selected/external_control_cmd` | TBD | Pass through control command with current mode. | -| `/external/selected/gear_cmd` | autoware_vehicle_msgs::msg::GearCommand | Pass through gear command with current mode. | -| `/external/selected/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | -| `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | -| `/external/selected/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | diff --git a/control/external_cmd_selector/package.xml b/control/external_cmd_selector/package.xml deleted file mode 100644 index b3520839e3b51..0000000000000 --- a/control/external_cmd_selector/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - external_cmd_selector - 0.1.0 - The external_cmd_selector package - Taiki Tanaka - Tomoya Kimura - Shumpei Wakabayashi - Fumiya Watanabe - Takamasa Horibe - Satoshi Ota - Takayuki Murooka - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_vehicle_msgs - diagnostic_updater - geometry_msgs - rclcpp - rclcpp_components - std_msgs - tier4_auto_msgs_converter - tier4_control_msgs - tier4_external_api_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/control/joy_controller/CMakeLists.txt b/control/joy_controller/CMakeLists.txt deleted file mode 100644 index f000c8c0909f4..0000000000000 --- a/control/joy_controller/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(joy_controller) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(joy_controller_node SHARED - src/joy_controller/joy_controller_node.cpp -) - -rclcpp_components_register_node(joy_controller_node - PLUGIN "joy_controller::AutowareJoyControllerNode" - EXECUTABLE joy_controller -) - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md deleted file mode 100644 index 73e3644df49ac..0000000000000 --- a/control/joy_controller/README.md +++ /dev/null @@ -1,125 +0,0 @@ -# joy_controller - -## Role - -`joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. - -## Usage - -### ROS 2 launch - -```bash -# With default config (ds4) -ros2 launch joy_controller joy_controller.launch.xml - -# Default config but select from the existing parameter files -ros2 launch joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox - -# Override the param file -ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml -``` - -## Input / Output - -### Input topics - -| Name | Type | Description | -| ------------------ | ----------------------- | --------------------------------- | -| `~/input/joy` | sensor_msgs::msg::Joy | joy controller command | -| `~/input/odometry` | nav_msgs::msg::Odometry | ego vehicle odometry to get twist | - -### Output topics - -| Name | Type | Description | -| ----------------------------------- | --------------------------------------------------- | ---------------------------------------- | -| `~/output/control_command` | autoware_control_msgs::msg::Control | lateral and longitudinal control command | -| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | -| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command | -| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command | -| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) | -| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat | -| `~/output/vehicle_engage` | autoware_vehicle_msgs::msg::Engage | vehicle engage | - -## Parameters - -| Parameter | Type | Description | -| ------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ | -| `joy_type` | string | joy controller type (default: DS4) | -| `update_rate` | double | update rate to publish control commands | -| `accel_ratio` | double | ratio to calculate acceleration (commanded acceleration is ratio \* operation) | -| `brake_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | -| `steer_ratio` | double | ratio to calculate deceleration (commanded steer is ratio \* operation) | -| `steering_angle_velocity` | double | steering angle velocity for operation | -| `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | -| `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | -| `raw_control` | bool | skip input odometry if true | -| `velocity_gain` | double | ratio to calculate velocity by acceleration | -| `max_forward_velocity` | double | absolute max velocity to go forward | -| `max_backward_velocity` | double | absolute max velocity to go backward | -| `backward_accel_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | - -## P65 Joystick Key Map - -| Action | Button | -| -------------------- | --------------------- | -| Acceleration | R2 | -| Brake | L2 | -| Steering | Left Stick Left Right | -| Shift up | Cursor Up | -| Shift down | Cursor Down | -| Shift Drive | Cursor Left | -| Shift Reverse | Cursor Right | -| Turn Signal Left | L1 | -| Turn Signal Right | R1 | -| Clear Turn Signal | A | -| Gate Mode | B | -| Emergency Stop | Select | -| Clear Emergency Stop | Start | -| Autoware Engage | X | -| Autoware Disengage | Y | -| Vehicle Engage | PS | -| Vehicle Disengage | Right Trigger | - -## DS4 Joystick Key Map - -| Action | Button | -| -------------------- | -------------------------- | -| Acceleration | R2, ×, or Right Stick Up | -| Brake | L2, □, or Right Stick Down | -| Steering | Left Stick Left Right | -| Shift up | Cursor Up | -| Shift down | Cursor Down | -| Shift Drive | Cursor Left | -| Shift Reverse | Cursor Right | -| Turn Signal Left | L1 | -| Turn Signal Right | R1 | -| Clear Turn Signal | SHARE | -| Gate Mode | OPTIONS | -| Emergency Stop | PS | -| Clear Emergency Stop | PS | -| Autoware Engage | ○ | -| Autoware Disengage | ○ | -| Vehicle Engage | △ | -| Vehicle Disengage | △ | - -## XBOX Joystick Key Map - -| Action | Button | -| -------------------- | --------------------- | -| Acceleration | RT | -| Brake | LT | -| Steering | Left Stick Left Right | -| Shift up | Cursor Up | -| Shift down | Cursor Down | -| Shift Drive | Cursor Left | -| Shift Reverse | Cursor Right | -| Turn Signal Left | LB | -| Turn Signal Right | RB | -| Clear Turn Signal | A | -| Gate Mode | B | -| Emergency Stop | View | -| Clear Emergency Stop | Menu | -| Autoware Engage | X | -| Autoware Disengage | Y | -| Vehicle Engage | Left Stick Button | -| Vehicle Disengage | Right Stick Button | diff --git a/control/joy_controller/launch/joy_controller_param_selection.launch.xml b/control/joy_controller/launch/joy_controller_param_selection.launch.xml deleted file mode 100644 index d8e3d0bfe8b25..0000000000000 --- a/control/joy_controller/launch/joy_controller_param_selection.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/control/joy_controller/package.xml b/control/joy_controller/package.xml deleted file mode 100644 index f7a5ed805b8ad..0000000000000 --- a/control/joy_controller/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - joy_controller - 0.1.0 - The joy_controller package - Taiki Tanaka - Tomoya Kimura - Shumpei Wakabayashi - Fumiya Watanabe - Takamasa Horibe - Satoshi Ota - Takayuki Murooka - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_control_msgs - autoware_vehicle_msgs - geometry_msgs - joy - nav_msgs - rclcpp - rclcpp_components - sensor_msgs - std_srvs - tier4_api_utils - tier4_control_msgs - tier4_external_api_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/control/lane_departure_checker/CMakeLists.txt b/control/lane_departure_checker/CMakeLists.txt deleted file mode 100644 index 874670c478240..0000000000000 --- a/control/lane_departure_checker/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(lane_departure_checker) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -include_directories( - include - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -ament_auto_add_library(lane_departure_checker SHARED - src/lane_departure_checker_node/lane_departure_checker.cpp - src/lane_departure_checker_node/lane_departure_checker_node.cpp -) - -rclcpp_components_register_node(lane_departure_checker - PLUGIN "lane_departure_checker::LaneDepartureCheckerNode" - EXECUTABLE lane_departure_checker_node -) - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml deleted file mode 100644 index 73955613d21a9..0000000000000 --- a/control/lane_departure_checker/package.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - lane_departure_checker - 0.1.0 - The lane_departure_checker package - Kyoichi Sugahara - Makoto Kurihara - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_map_msgs - autoware_planning_msgs - boost - diagnostic_updater - eigen - geometry_msgs - lanelet2_extension - motion_utils - nav_msgs - rclcpp - rclcpp_components - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - vehicle_info_util - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt deleted file mode 100644 index e4a3683ea1fdc..0000000000000 --- a/control/mpc_lateral_controller/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(mpc_lateral_controller) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(steering_offset_lib SHARED - src/steering_offset/steering_offset.cpp -) - -set(MPC_LAT_CON_LIB ${PROJECT_NAME}_lib) -ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED - src/mpc_lateral_controller.cpp - src/lowpass_filter.cpp - src/steering_predictor.cpp - src/mpc.cpp - src/mpc_trajectory.cpp - src/mpc_utils.cpp - src/qp_solver/qp_solver_osqp.cpp - src/qp_solver/qp_solver_unconstraint_fast.cpp - src/vehicle_model/vehicle_model_bicycle_dynamics.cpp - src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp - src/vehicle_model/vehicle_model_bicycle_kinematics.cpp - src/vehicle_model/vehicle_model_interface.cpp -) -target_link_libraries(${MPC_LAT_CON_LIB} steering_offset_lib) - -if(BUILD_TESTING) - set(TEST_LAT_SOURCES - test/test_mpc.cpp - test/test_mpc_utils.cpp - test/test_lowpass_filter.cpp - ) - set(TEST_LATERAL_CONTROLLER_EXE test_lateral_controller) - ament_add_ros_isolated_gtest(${TEST_LATERAL_CONTROLLER_EXE} ${TEST_LAT_SOURCES}) - target_link_libraries(${TEST_LATERAL_CONTROLLER_EXE} ${MPC_LAT_CON_LIB}) -endif() - -ament_auto_package(INSTALL_TO_SHARE - param -) diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md deleted file mode 100644 index 3ad116929b50c..0000000000000 --- a/control/mpc_lateral_controller/README.md +++ /dev/null @@ -1,266 +0,0 @@ -# MPC Lateral Controller - -This is the design document for the lateral controller node -in the `trajectory_follower_node` package. - -## Purpose / Use cases - - - - -This node is used to general lateral control commands (steering angle and steering rate) -when following a path. - -## Design - - - - -The node uses an implementation of linear model predictive control (MPC) for accurate path tracking. -The MPC uses a model of the vehicle to simulate the trajectory resulting from the control command. -The optimization of the control command is formulated as a Quadratic Program (QP). - -Different vehicle models are implemented: - -- kinematics : bicycle kinematics model with steering 1st-order delay. -- kinematics_no_delay : bicycle kinematics model without steering delay. -- dynamics : bicycle dynamics model considering slip angle. - The kinematics model is being used by default. Please see the reference [1] for more details. - -For the optimization, a Quadratic Programming (QP) solver is used and two options are currently implemented: - - - -- unconstraint_fast : use least square method to solve unconstraint QP with eigen. -- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) - algorithm (for more details see the related papers at - the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section): - -### Filtering - -Filtering is required for good noise reduction. -A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is employed for processing the yaw and lateral errors, which are used as inputs for the MPC, as well as for refining the output steering angle. -Other filtering methods can be considered as long as the noise reduction performances are good -enough. -The moving average filter for example is not suited and can yield worse results than without any -filtering. - -## Assumptions / Known limits - - - -The tracking is not accurate if the first point of the reference trajectory is at or in front of the current ego pose. - -## Inputs / Outputs / API - - - - -### Inputs - -Set the following from the [controller_node](../trajectory_follower_node/README.md) - -- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. -- `nav_msgs/Odometry`: current odometry -- `autoware_vehicle_msgs/SteeringReport`: current steering - -### Outputs - -Return LateralOutput which contains the following to the controller node - -- `autoware_control_msgs/Lateral` -- LateralSyncData - - steer angle convergence - -### MPC class - -The `MPC` class (defined in `mpc.hpp`) provides the interface with the MPC algorithm. -Once a vehicle model, a QP solver, and the reference trajectory to follow have been set -(using `setVehicleModel()`, `setQPSolver()`, `setReferenceTrajectory()`), a lateral control command -can be calculated by providing the current steer, velocity, and pose to function `calculateMPC()`. - -### Parameter description - -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. - -#### 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 (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] | [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 | -| :------------ | :----- | :------------------------------------------ | :------------ | -| cg_to_front_m | double | distance from baselink to the front axle[m] | 1.228 | -| cg_to_rear_m | double | distance from baselink to the rear axle [m] | 1.5618 | -| mass_fl | double | mass applied to front left tire [kg] | 600 | -| mass_fr | double | mass applied to front right tire [kg] | 600 | -| mass_rl | double | mass applied to rear left tire [kg] | 600 | -| mass_rr | double | mass applied to rear right tire [kg] | 600 | -| cf | double | front cornering power [N/rad] | 155494.663 | -| cr | double | rear cornering power [N/rad] | 155494.663 | - -### How to tune MPC parameters - -#### Set kinematics information - -First, it's important to set the appropriate parameters for vehicle kinematics. This includes parameters like `wheelbase`, which represents the distance between the front and rear wheels, and `max_steering_angle`, which indicates the maximum tire steering angle. These parameters should be set in the `vehicle_info.param.yaml`. - -#### Set dynamics information - -Next, you need to set the proper parameters for the dynamics model. These include the time constant `steering_tau` and time delay `steering_delay` for steering dynamics, and the maximum acceleration `mpc_acceleration_limit` and the time constant `mpc_velocity_time_constant` for velocity dynamics. - -#### Confirmation of the input information - -It's also important to make sure the input information is accurate. Information such as the velocity of the center of the rear wheel [m/s] and the steering angle of the tire [rad] is required. Please note that there have been frequent reports of performance degradation due to errors in input information. For instance, there are cases where the velocity of the vehicle is offset due to an unexpected difference in tire radius, or the tire angle cannot be accurately measured due to a deviation in the steering gear ratio or midpoint. It is suggested to compare information from multiple sensors (e.g., integrated vehicle speed and GNSS position, steering angle and IMU angular velocity), and ensure the input information for MPC is appropriate. - -#### MPC weight tuning - -Then, tune the weights of the MPC. One simple approach of tuning is to keep the weight for the lateral deviation (`weight_lat_error`) constant, and vary the input weight (`weight_steering_input`) while observing the trade-off between steering oscillation and control accuracy. - -Here, `weight_lat_error` acts to suppress the lateral error in path following, while `weight_steering_input` works to adjust the steering angle to a standard value determined by the path's curvature. When `weight_lat_error` is large, the steering moves significantly to improve accuracy, which can cause oscillations. On the other hand, when `weight_steering_input` is large, the steering doesn't respond much to tracking errors, providing stable driving but potentially reducing tracking accuracy. - -The steps are as follows: - -1. Set `weight_lat_error` = 0.1, `weight_steering_input` = 1.0 and other weights to 0. -2. If the vehicle oscillates when driving, set `weight_steering_input` larger. -3. If the tracking accuracy is low, set `weight_steering_input` smaller. - -If you want to adjust the effect only in the high-speed range, you can use `weight_steering_input_squared_vel`. This parameter corresponds to the steering weight in the high-speed range. - -#### Descriptions for weights - -- `weight_lat_error`: Reduce lateral tracking error. This acts like P gain in PID. -- `weight_heading_error`: Make a drive straight. This acts like D gain in PID. -- `weight_heading_error_squared_vel_coeff` : Make a drive straight in high speed range. -- `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. - -#### Other tips for tuning - -Here are some tips for adjusting other parameters: - -- In theory, increasing terminal weights, `weight_terminal_lat_error` and `weight_terminal_heading_error`, can enhance the tracking stability. This method sometimes proves effective. -- A larger `prediction_horizon` and a smaller `prediction_sampling_time` are efficient for tracking performance. However, these come at the cost of higher computational costs. -- If you want to modify the weight according to the trajectory curvature (for instance, when you're driving on a sharp curve and want a larger weight), use `mpc_low_curvature_thresh_curvature` and adjust `mpc_low_curvature_weight_**` weights. -- If you want to adjust the steering rate limit based on the vehicle speed and trajectory curvature, you can modify the values of `steer_rate_lim_dps_list_by_curvature`, `curvature_list_for_steer_rate_lim`, `steer_rate_lim_dps_list_by_velocity`, `velocity_list_for_steer_rate_lim`. By doing this, you can enforce the steering rate limit during high-speed driving or relax it while curving. -- In case your target curvature appears jagged, adjusting `curvature_smoothing` becomes critically important for accurate curvature calculations. A larger value yields a smooth curvature calculation which reduces noise but can cause delay in feedforward computation and potentially degrade performance. -- Adjusting the `steering_lpf_cutoff_hz` value can also be effective to forcefully reduce computational noise. This refers to the cutoff frequency in the second order Butterworth filter installed in the final layer. The smaller the cutoff frequency, the stronger the noise reduction, but it also induce operation delay. -- If the vehicle consistently deviates laterally from the trajectory, it's most often due to the offset of the steering sensor or self-position estimation. It's preferable to eliminate these biases before inputting into MPC, but it's also possible to remove this bias within MPC. To utilize this, set `enable_auto_steering_offset_removal` to true and activate the steering offset remover. The steering offset estimation logic works when driving at high speeds with the steering close to the center, applying offset removal. -- If the onset of steering in curves is late, it's often due to incorrect delay time and time constant in the steering model. Please recheck the values of `input_delay` and `vehicle_model_steer_tau`. Additionally, as a part of its debug information, MPC outputs the current steering angle assumed by the MPC model, so please check if that steering angle matches the actual one. - -## References / External links - - - -- [1] Jarrod M. Snider, "Automatic Steering Methods for Autonomous Automobile Path Tracking", - Robotics Institute, Carnegie Mellon University, February 2009. - -## Related issues - - diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp deleted file mode 100644 index 227bd0594312b..0000000000000 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright 2018-2021 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 MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ -#define MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ - -#include -#include -#include -#include - -namespace autoware::motion::control::mpc_lateral_controller -{ -/** - * @brief 2nd-order Butterworth Filter - * reference : S. Butterworth, "On the Theory of Filter Amplifier", Experimental wireless, 1930. - */ -class Butterworth2dFilter -{ -private: - double m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_y2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_u1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_u2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_a1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_a2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_b0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_b1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - double m_b2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time - -public: - /** - * @brief constructor with initialization - * @param [in] dt sampling time - * @param [in] f_cutoff_hz cutoff frequency [Hz] - */ - explicit Butterworth2dFilter(double dt = 0.01, double f_cutoff_hz = 5.0); - - /** - * @brief destructor - */ - ~Butterworth2dFilter(); - - /** - * @brief constructor - * @param [in] dt sampling time - * @param [in] f_cutoff_hz cutoff frequency [Hz] - */ - void initialize(const double & dt, const double & f_cutoff_hz); - - /** - * @brief filtering (call this function at each sampling time with input) - * @param [in] u scalar input for filter - * @return filtered scalar value - */ - double filter(const double & u); - - /** - * @brief filtering for time-series data - * @param [in] t time-series data for input vector - * @param [out] u object vector - */ - void filt_vector(const std::vector & t, std::vector & u) const; - - /** - * @brief filtering for time-series data from both forward-backward direction for zero phase delay - * @param [in] t time-series data for input vector - * @param [out] u object vector - */ - void filtfilt_vector( - const std::vector & t, - std::vector & u) const; // filtering forward and backward direction - - /** - * @brief get filter coefficients - * @param [out] coeffs coefficients of filter [a0, a1, a2, b0, b1, b2]. - */ - void getCoefficients(std::vector & coeffs) const; -}; - -/** - * @brief Move Average Filter - */ -namespace MoveAverageFilter -{ -/** - * @brief filtering vector - * @param [in] num index distance for moving average filter - * @param [out] u object vector - */ -bool filt_vector(const int num, std::vector & u); -} // namespace MoveAverageFilter -} // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml deleted file mode 100644 index 000bddc65aa1f..0000000000000 --- a/control/mpc_lateral_controller/package.xml +++ /dev/null @@ -1,48 +0,0 @@ - - - - mpc_lateral_controller - 1.0.0 - MPC-based lateral controller - - Takamasa Horibe - Takayuki Murooka - - Apache 2.0 - - Takamasa Horibe - Maxime CLEMENT - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - autoware_control_msgs - autoware_planning_msgs - autoware_vehicle_msgs - diagnostic_msgs - diagnostic_updater - eigen - geometry_msgs - interpolation - motion_utils - osqp_interface - rclcpp - rclcpp_components - std_msgs - tf2 - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - trajectory_follower_base - vehicle_info_util - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp index a65d818056bd7..efaefd765e793 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -15,8 +15,8 @@ #ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ #define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ +#include #include -#include #include #include @@ -75,7 +75,7 @@ class ObstacleCollisionChecker private: Param param_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; //! This function assumes the input trajectory is sampled dense enough static autoware_planning_msgs::msg::Trajectory resampleTrajectory( @@ -86,7 +86,7 @@ class ObstacleCollisionChecker static std::vector createVehicleFootprints( const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml index 078993d064064..f82384534e040 100755 --- a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml +++ b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index ef7560755a122..795302abe75f4 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -19,6 +19,7 @@ autoware_cmake autoware_planning_msgs + autoware_vehicle_info_utils boost diagnostic_updater eigen @@ -33,7 +34,6 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index 6887be4cedd77..a844fe6a50cbf 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -83,7 +83,7 @@ double calcBrakingDistance( namespace obstacle_collision_checker { ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) -: vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) +: vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { } @@ -188,7 +188,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( std::vector ObstacleCollisionChecker::createVehicleFootprints( const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { // Create vehicle footprint in base_link coordinate const auto local_vehicle_footprint = vehicle_info.createFootprint(param.footprint_margin); diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index cfde4ee849728..402123c81d361 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -14,10 +14,10 @@ #include "obstacle_collision_checker/obstacle_collision_checker_node.hpp" +#include #include #include #include -#include #include #include diff --git a/control/operation_mode_transition_manager/CMakeLists.txt b/control/operation_mode_transition_manager/CMakeLists.txt deleted file mode 100644 index 9c8ca29fdc277..0000000000000 --- a/control/operation_mode_transition_manager/CMakeLists.txt +++ /dev/null @@ -1,47 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(operation_mode_transition_manager) - -find_package(autoware_cmake REQUIRED) -autoware_package() -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -ament_auto_add_library(operation_mode_transition_manager_node SHARED - src/compatibility.cpp - src/data.cpp - src/node.cpp - src/state.cpp -) - -rclcpp_components_register_node(operation_mode_transition_manager_node - PLUGIN "operation_mode_transition_manager::OperationModeTransitionManager" - EXECUTABLE operation_mode_transition_manager_exe -) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "msg/OperationModeTransitionManagerDebug.msg" - DEPENDENCIES builtin_interfaces -) - -# to use same package defined message -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(operation_mode_transition_manager_node - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(operation_mode_transition_manager_node "${cpp_typesupport_target}") -endif() - - -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/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md deleted file mode 100644 index 008d092565ba4..0000000000000 --- a/control/operation_mode_transition_manager/README.md +++ /dev/null @@ -1,138 +0,0 @@ -# operation_mode_transition_manager - -## Purpose / Use cases - -This module is responsible for managing the different modes of operation for the Autoware system. The possible modes are: - -- `Autonomous`: the vehicle is fully controlled by the autonomous driving system -- `Local`: the vehicle is controlled by a physically connected control system such as a joy stick -- `Remote`: the vehicle is controlled by a remote controller -- `Stop`: the vehicle is stopped and there is no active control system. - -There is also an `In Transition` state that occurs during each mode transitions. During this state, the transition to the new operator is not yet complete, and the previous operator is still responsible for controlling the system until the transition is complete. Some actions may be restricted during the `In Transition` state, such as sudden braking or steering. (This is restricted by the `vehicle_cmd_gate`). - -### Features - -- Transit mode between `Autonomous`, `Local`, `Remote` and `Stop` based on the indication command. -- Check whether the each transition is available (safe or not). -- Limit some sudden motion control in `In Transition` mode (this is done with `vehicle_cmd_gate` feature). -- Check whether the transition is completed. - -- Transition between the `Autonomous`, `Local`, `Remote`, and `Stop` modes based on the indicated command. -- Determine whether each transition is safe to execute. -- Restrict certain sudden motion controls during the `In Transition` mode (using the `vehicle_cmd_gate` feature). -- Verify that the transition is complete. - -## 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 - -### Inputs - -For the mode transition: - -- /system/operation_mode/change_autoware_control [`tier4_system_msgs/srv/ChangeAutowareControl`]: change operation mode to Autonomous -- /system/operation_mode/change_operation_mode [`tier4_system_msgs/srv/ChangeOperationMode`]: change operation mode - -For the transition availability/completion check: - -- /control/command/control_cmd [`autoware_control_msgs/msg/Control`]: vehicle control signal -- /localization/kinematic_state [`nav_msgs/msg/Odometry`]: ego vehicle state -- /planning/scenario_planning/trajectory [`autoware_planning_msgs/msg/Trajectory`]: planning trajectory -- /vehicle/status/control_mode [`autoware_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) -- /control/vehicle_cmd_gate/operation_mode [`autoware_adapi_v1_msgs/msg/OperationModeState`]: the operation mode in the `vehicle_cmd_gate`. (To be removed) - -For the backward compatibility (to be removed): - -- /api/autoware/get/engage [`autoware_vehicle_msgs/msg/Engage`] -- /control/current_gate_mode [`tier4_control_msgs/msg/GateMode`] -- /control/external_cmd_selector/current_selector_mode [`tier4_control_msgs/msg/ExternalCommandSelectorMode`] - -### Outputs - -- /system/operation_mode/state [`autoware_adapi_v1_msgs/msg/OperationModeState`]: to inform the current operation mode -- /control/operation_mode_transition_manager/debug_info [`operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug`]: detailed information about the operation mode transition - -- /control/gate_mode_cmd [`tier4_control_msgs/msg/GateMode`]: to change the `vehicle_cmd_gate` state to use its features (to be removed) -- /autoware/engage [`autoware_vehicle_msgs/msg/Engage`]: - -- /control/control_mode_request [`autoware_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) -- /control/external_cmd_selector/select_external_command [`tier4_control_msgs/srv/ExternalCommandSelect`]: - -## Parameters - -{{ json_to_markdown("control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json") }} - -| 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: - -| Name | Type | Description | Default value | -| :---------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `allow_autonomous_in_stopped` | `bool` | If true, autonomous transition is available when the vehicle is stopped even if other checks fail. | true | -| `dist_threshold` | `double` | the distance between the trajectory and ego vehicle must be within this distance for `Autonomous` transition. | 1.5 | -| `yaw_threshold` | `double` | the yaw angle between trajectory and ego vehicle must be within this threshold for `Autonomous` transition. | 0.524 | -| `speed_upper_threshold` | `double` | the velocity deviation between control command and ego vehicle must be within this threshold for `Autonomous` transition. | 10.0 | -| `speed_lower_threshold` | `double` | the velocity deviation between the control command and ego vehicle must be within this threshold for `Autonomous` transition. | -10.0 | -| `acc_threshold` | `double` | the control command acceleration must be less than this threshold for `Autonomous` transition. | 1.5 | -| `lateral_acc_threshold` | `double` | the control command lateral acceleration must be less than this threshold for `Autonomous` transition. | 1.0 | -| `lateral_acc_diff_threshold` | `double` | the lateral acceleration deviation between the control command must be less than this threshold for `Autonomous` transition. | 0.5 | - -For `stable_check` related parameters: - -| Name | Type | Description | Default value | -| :---------------------- | :------- | :-------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `duration` | `double` | the stable condition must be satisfied for this duration to complete the transition. | 0.1 | -| `dist_threshold` | `double` | the distance between the trajectory and ego vehicle must be within this distance to complete `Autonomous` transition. | 1.5 | -| `yaw_threshold` | `double` | the yaw angle between trajectory and ego vehicle must be within this threshold to complete `Autonomous` transition. | 0.262 | -| `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. -- This node should be merged to the `vehicle_cmd_gate` due to its strong connection. diff --git a/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml b/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml deleted file mode 100644 index 569d743ba2c4f..0000000000000 --- a/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml deleted file mode 100644 index 99439aa7e3ab7..0000000000000 --- a/control/operation_mode_transition_manager/package.xml +++ /dev/null @@ -1,39 +0,0 @@ - - operation_mode_transition_manager - 0.1.0 - The vehicle_cmd_gate package - Takamasa Horibe - Tomoya Kimura - Apache License 2.0 - - Takamasa Horibe - - autoware_cmake - rosidl_default_generators - - autoware_control_msgs - autoware_vehicle_msgs - component_interface_specs - component_interface_utils - geometry_msgs - motion_utils - rclcpp - rclcpp_components - std_srvs - tier4_autoware_utils - tier4_control_msgs - tier4_system_msgs - tier4_vehicle_msgs - vehicle_info_util - - ament_cmake_gtest - ament_lint_auto - autoware_lint_common - - rosidl_default_runtime - rosidl_interface_packages - - - ament_cmake - - diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp deleted file mode 100644 index 9c268102d7b67..0000000000000 --- a/control/operation_mode_transition_manager/src/node.cpp +++ /dev/null @@ -1,292 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "node.hpp" - -#include - -namespace operation_mode_transition_manager -{ - -OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::NodeOptions & options) -: Node("operation_mode_transition_manager", options), compatibility_(this) -{ - sub_control_mode_report_ = create_subscription( - "control_mode_report", 1, - [this](const ControlModeReport::SharedPtr msg) { control_mode_report_ = *msg; }); - - sub_gate_operation_mode_ = create_subscription( - "gate_operation_mode", 1, - [this](const OperationModeState::SharedPtr msg) { gate_operation_mode_ = *msg; }); - - cli_control_mode_ = create_client("control_mode_request"); - pub_debug_info_ = create_publisher("~/debug_info", 1); - - // component interface - { - const auto node = component_interface_utils::NodeAdaptor(this); - node.init_srv( - srv_autoware_control_, this, &OperationModeTransitionManager::onChangeAutowareControl); - node.init_srv( - srv_operation_mode_, this, &OperationModeTransitionManager::onChangeOperationMode); - node.init_pub(pub_operation_mode_); - } - - // timer - { - const auto period_ns = rclcpp::Rate(declare_parameter("frequency_hz")).period(); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&OperationModeTransitionManager::onTimer, this)); - } - - // initialize state - current_mode_ = OperationMode::STOP; - transition_ = nullptr; - gate_operation_mode_.mode = OperationModeState::UNKNOWN; - gate_operation_mode_.is_in_transition = false; - control_mode_report_.mode = ControlModeReport::NO_COMMAND; - transition_timeout_ = declare_parameter("transition_timeout"); - { - // check `transition_timeout` value - const auto stable_duration = declare_parameter("stable_check.duration"); - const double TIMEOUT_MARGIN = 0.5; - if (transition_timeout_ < stable_duration + TIMEOUT_MARGIN) { - transition_timeout_ = stable_duration + TIMEOUT_MARGIN; - RCLCPP_WARN( - get_logger(), "`transition_timeout` must be somewhat larger than `stable_check.duration`"); - RCLCPP_WARN_STREAM(get_logger(), "transition_timeout is set to " << transition_timeout_); - } - } - - // modes - modes_[OperationMode::STOP] = std::make_unique(); - modes_[OperationMode::AUTONOMOUS] = std::make_unique(this); - modes_[OperationMode::LOCAL] = std::make_unique(); - modes_[OperationMode::REMOTE] = std::make_unique(); -} - -void OperationModeTransitionManager::onChangeAutowareControl( - const ChangeAutowareControlAPI::Service::Request::SharedPtr request, - const ChangeAutowareControlAPI::Service::Response::SharedPtr response) -{ - if (request->autoware_control) { - // Treat as a transition to the current operation mode. - changeOperationMode(std::nullopt); - } else { - // Allow mode transition to complete without canceling. - compatibility_transition_ = std::nullopt; - transition_.reset(); - changeControlMode(ControlModeCommand::Request::MANUAL); - } - response->status.success = true; -} - -void OperationModeTransitionManager::onChangeOperationMode( - const ChangeOperationModeAPI::Service::Request::SharedPtr request, - const ChangeOperationModeAPI::Service::Response::SharedPtr response) -{ - const auto mode = toEnum(*request); - if (!mode) { - throw component_interface_utils::ParameterError("The operation mode is invalid."); - } - changeOperationMode(mode.value()); - response->status.success = true; -} - -void OperationModeTransitionManager::changeControlMode(ControlModeCommandType mode) -{ - const auto callback = [this](rclcpp::Client::SharedFuture future) { - if (!future.get()->success) { - RCLCPP_WARN(get_logger(), "Autonomous mode change was rejected."); - if (transition_) { - transition_->is_engage_failed = true; - } - } - }; - - const auto request = std::make_shared(); - request->stamp = now(); - request->mode = mode; - cli_control_mode_->async_send_request(request, callback); -} - -void OperationModeTransitionManager::changeOperationMode(std::optional request_mode) -{ - // NOTE: If request_mode is nullopt, indicates to enable autoware control - - const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; - const bool request_control = request_mode ? false : true; - - if (current_mode_ == request_mode) { - throw component_interface_utils::NoEffectWarning("The mode is the same as the current."); - } - - if (current_control && request_control) { - throw component_interface_utils::NoEffectWarning("Autoware already controls the vehicle."); - } - - // TODO(Takagi, Isamu): Consider mode change request during transition. - if (transition_ && request_mode != OperationMode::STOP) { - throw component_interface_utils::ServiceException( - ServiceResponse::ERROR_IN_TRANSITION, "The mode transition is in progress."); - } - - // Enter transition mode if the vehicle is being or will be controlled by Autoware. - if (current_control || request_control) { - if (!available_mode_change_[request_mode.value_or(current_mode_)]) { - throw component_interface_utils::ServiceException( - ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); - } - if (request_control) { - transition_ = std::make_unique(now(), request_control, std::nullopt); - } else { - transition_ = std::make_unique(now(), request_control, current_mode_); - } - } - compatibility_transition_ = now(); - current_mode_ = request_mode.value_or(current_mode_); -} - -void OperationModeTransitionManager::cancelTransition() -{ - const auto & previous = transition_->previous; - if (previous) { - compatibility_transition_ = now(); - current_mode_ = previous.value(); - } else { - compatibility_transition_ = std::nullopt; - changeControlMode(ControlModeCommand::Request::MANUAL); - } - transition_.reset(); -} - -void OperationModeTransitionManager::processTransition() -{ - const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; - - // Check timeout. - if (transition_timeout_ < (now() - transition_->time).seconds()) { - return cancelTransition(); - } - - // Check engage failure. - if (transition_->is_engage_failed) { - return cancelTransition(); - } - - // Check override. - if (current_control) { - transition_->is_engage_completed = true; - } else { - if (transition_->is_engage_completed) { - return cancelTransition(); - } - } - - // Check reflection of mode change to the compatible interface. - if (current_mode_ != compatibility_.get_mode()) { - return; - } - - // Check completion when engaged, otherwise engage after the gate reflects transition. - if (current_control) { - if (modes_.at(current_mode_)->isModeChangeCompleted()) { - return transition_.reset(); - } - } else { - if (transition_->is_engage_requested && gate_operation_mode_.is_in_transition) { - transition_->is_engage_requested = false; - return changeControlMode(ControlModeCommand::Request::AUTONOMOUS); - } - } -} - -void OperationModeTransitionManager::onTimer() -{ - for (const auto & [type, mode] : modes_) { - mode->update(current_mode_ == type && transition_); - } - - for (const auto & [type, mode] : modes_) { - available_mode_change_[type] = mode->isModeChangeAvailable(); - } - - // Check sync timeout to the compatible interface. - if (compatibility_transition_) { - if (compatibility_timeout_ < (now() - compatibility_transition_.value()).seconds()) { - compatibility_transition_ = std::nullopt; - } - } - - // Reflects the mode when changed by the compatible interface. - if (compatibility_transition_) { - compatibility_.set_mode(current_mode_); - } else { - current_mode_ = compatibility_.get_mode().value_or(current_mode_); - } - - // Reset sync timeout when it is completed. - if (compatibility_transition_) { - if (current_mode_ == compatibility_.get_mode()) { - compatibility_transition_ = std::nullopt; - } - } - - if (transition_) { - processTransition(); - } - - publishData(); -} - -void OperationModeTransitionManager::publishData() -{ - const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; - const auto time = now(); - - OperationModeStateAPI::Message state; - state.mode = toMsg(current_mode_); - state.is_autoware_control_enabled = current_control; - state.is_in_transition = transition_ ? true : false; - state.is_stop_mode_available = available_mode_change_[OperationMode::STOP]; - state.is_autonomous_mode_available = available_mode_change_[OperationMode::AUTONOMOUS]; - state.is_local_mode_available = available_mode_change_[OperationMode::LOCAL]; - state.is_remote_mode_available = available_mode_change_[OperationMode::REMOTE]; - - if (prev_state_ != state) { - prev_state_ = state; - state.stamp = time; - pub_operation_mode_->publish(state); - } - - const auto status_str = [&]() { - if (!current_control) return "DISENGAGE (autoware mode = " + toString(current_mode_) + ")"; - if (transition_) - return toString(current_mode_) + " (in transition from " + toString(transition_->previous) + - ")"; - return toString(current_mode_); - }(); - - ModeChangeBase::DebugInfo debug_info = modes_.at(OperationMode::AUTONOMOUS)->getDebugInfo(); - debug_info.stamp = time; - debug_info.status = status_str; - debug_info.in_autoware_control = current_control; - debug_info.in_transition = transition_ ? true : false; - pub_debug_info_->publish(debug_info); -} - -} // namespace operation_mode_transition_manager - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(operation_mode_transition_manager::OperationModeTransitionManager) diff --git a/control/operation_mode_transition_manager/src/node.hpp b/control/operation_mode_transition_manager/src/node.hpp deleted file mode 100644 index c2d761fcf138c..0000000000000 --- a/control/operation_mode_transition_manager/src/node.hpp +++ /dev/null @@ -1,81 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NODE_HPP_ -#define NODE_HPP_ - -#include "compatibility.hpp" -#include "state.hpp" - -#include -#include -#include - -#include -#include -#include - -namespace operation_mode_transition_manager -{ - -class OperationModeTransitionManager : public rclcpp::Node -{ -public: - explicit OperationModeTransitionManager(const rclcpp::NodeOptions & options); - -private: - using ChangeAutowareControlAPI = system_interface::ChangeAutowareControl; - using ChangeOperationModeAPI = system_interface::ChangeOperationMode; - using OperationModeStateAPI = system_interface::OperationModeState; - component_interface_utils::Service::SharedPtr srv_autoware_control_; - component_interface_utils::Service::SharedPtr srv_operation_mode_; - component_interface_utils::Publisher::SharedPtr pub_operation_mode_; - void onChangeAutowareControl( - const ChangeAutowareControlAPI::Service::Request::SharedPtr request, - const ChangeAutowareControlAPI::Service::Response::SharedPtr response); - void onChangeOperationMode( - const ChangeOperationModeAPI::Service::Request::SharedPtr request, - const ChangeOperationModeAPI::Service::Response::SharedPtr response); - - using ControlModeCommandType = ControlModeCommand::Request::_mode_type; - rclcpp::Subscription::SharedPtr sub_control_mode_report_; - rclcpp::Subscription::SharedPtr sub_gate_operation_mode_; - rclcpp::Client::SharedPtr cli_control_mode_; - rclcpp::Publisher::SharedPtr pub_debug_info_; - rclcpp::TimerBase::SharedPtr timer_; - void onTimer(); - void publishData(); - void changeControlMode(ControlModeCommandType mode); - void changeOperationMode(std::optional mode); - void cancelTransition(); - void processTransition(); - - double transition_timeout_; - OperationMode current_mode_; - std::unique_ptr transition_; - std::unordered_map> modes_; - std::unordered_map available_mode_change_; - OperationModeState gate_operation_mode_; - ControlModeReport control_mode_report_; - - std::optional prev_state_; - - static constexpr double compatibility_timeout_ = 1.0; - Compatibility compatibility_; - std::optional compatibility_transition_; -}; - -} // namespace operation_mode_transition_manager - -#endif // NODE_HPP_ diff --git a/control/operation_mode_transition_manager/src/util.hpp b/control/operation_mode_transition_manager/src/util.hpp deleted file mode 100644 index e3fa20aaafd74..0000000000000 --- a/control/operation_mode_transition_manager/src/util.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef UTIL_HPP_ -#define UTIL_HPP_ - -#include "operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" - -using DebugInfo = operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; - -void setAllOk(DebugInfo & debug_info) -{ - debug_info.is_all_ok = true; - debug_info.engage_allowed_for_stopped_vehicle = true; - debug_info.large_acceleration_ok = true; - debug_info.large_lateral_acceleration_diff_ok = true; - debug_info.large_lateral_acceleration_ok = true; - debug_info.lateral_deviation_ok = true; - debug_info.speed_lower_deviation_ok = true; - debug_info.speed_upper_deviation_ok = true; - debug_info.stop_ok = true; - debug_info.trajectory_available_ok = true; - debug_info.yaw_deviation_ok = true; -} - -#endif // UTIL_HPP_ diff --git a/control/pid_longitudinal_controller/CMakeLists.txt b/control/pid_longitudinal_controller/CMakeLists.txt deleted file mode 100644 index 3e7a992b15e00..0000000000000 --- a/control/pid_longitudinal_controller/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(pid_longitudinal_controller) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -set(PID_LON_CON_LIB ${PROJECT_NAME}_lib) -ament_auto_add_library(${PID_LON_CON_LIB} SHARED - src/pid_longitudinal_controller.cpp - src/pid.cpp - src/smooth_stop.cpp - src/longitudinal_controller_utils.cpp -) - -if(BUILD_TESTING) - set(TEST_LON_SOURCES - test/test_pid.cpp - test/test_smooth_stop.cpp - test/test_longitudinal_controller_utils.cpp - ) - set(TEST_LONGITUDINAL_CONTROLLER_EXE test_longitudinal_controller) - ament_add_ros_isolated_gtest(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${TEST_LON_SOURCES}) - target_link_libraries(${TEST_LONGITUDINAL_CONTROLLER_EXE} ${PID_LON_CON_LIB}) -endif() - -ament_auto_package( - INSTALL_TO_SHARE - param -) diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md deleted file mode 100644 index 4e20cb27fe66a..0000000000000 --- a/control/pid_longitudinal_controller/README.md +++ /dev/null @@ -1,266 +0,0 @@ -# PID Longitudinal Controller - -## Purpose / Use cases - -The longitudinal_controller computes the target acceleration to achieve the target velocity set at each point of the target trajectory using a feed-forward/back control. - -It also contains a slope force correction that takes into account road slope information, and a delay compensation function. -It is assumed that the target acceleration calculated here will be properly realized by the vehicle interface. - -Note that the use of this module is not mandatory for Autoware if the vehicle supports the "target speed" interface. - -## Design / Inner-workings / Algorithms - -### States - -This module has four state transitions as shown below in order to handle special processing in a specific situation. - -- **DRIVE** - - Executes target velocity tracking by PID control. - - It also applies the delay compensation and slope compensation. -- **STOPPING** - - Controls the motion just before stopping. - - Special sequence is performed to achieve accurate and smooth stopping. -- **STOPPED** - - Performs operations in the stopped state (e.g. brake hold) -- **EMERGENCY**. - - Enters an emergency state when certain conditions are met (e.g., when the vehicle has crossed a certain distance of a stop line). - - The recovery condition (whether or not to keep emergency state until the vehicle completely stops) or the deceleration in the emergency state are defined by parameters. - -The state transition diagram is shown below. - -![LongitudinalControllerStateTransition](./media/LongitudinalControllerStateTransition.drawio.svg) - -### Logics - -#### Control Block Diagram - -![LongitudinalControllerDiagram](./media/LongitudinalControllerDiagram.drawio.svg) - -#### FeedForward (FF) - -The reference acceleration set in the trajectory and slope compensation terms are output as a feedforward. Under ideal conditions with no modeling error, this FF term alone should be sufficient for velocity tracking. - -Tracking errors causing modeling or discretization errors are removed by the feedback control (now using PID). - -##### Brake keeping - -From the viewpoint of ride comfort, stopping with 0 acceleration is important because it reduces the impact of braking. However, if the target acceleration when stopping is 0, the vehicle may cross over the stop line or accelerate a little in front of the stop line due to vehicle model error or gradient estimation error. - -For reliable stopping, the target acceleration calculated by the FeedForward system is limited to a negative acceleration when stopping. - -![BrakeKeepingDiagram](./media/BrakeKeeping.drawio.svg) - -#### Slope compensation - -Based on the slope information, a compensation term is added to the target acceleration. - -There are two sources of the slope information, which can be switched by a parameter. - -- Pitch of the estimated ego-pose (default) - - Calculates the current slope from the pitch angle of the estimated ego-pose - - Pros: Easily available - - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. -- Z coordinate on the trajectory - - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory - - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained - - Pros: Can be used in combination with delay compensation (not yet implemented) - - Cons: z-coordinates of high-precision map is needed. - - Cons: Does not support free space planning (for now) - -**Notation:** This function works correctly only in a vehicle system that does not have acceleration feedback in the low-level control system. - -This compensation adds gravity correction to the target acceleration, resulting in an output value that is no longer equal to the target acceleration that the autonomous driving system desires. Therefore, it conflicts with the role of the acceleration feedback in the low-level controller. -For instance, if the vehicle is attempting to start with an acceleration of `1.0 m/s^2` and a gravity correction of `-1.0 m/s^2` is applied, the output value will be `0`. If this output value is mistakenly treated as the target acceleration, the vehicle will not start. - -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. - -This PID control calculates the target acceleration from the deviation between the current ego-velocity and the target velocity. - -This PID logic has a maximum value for the output of each term. This is to prevent the following: - -- Large integral terms may cause unintended behavior by users. -- Unintended noise may cause the output of the derivative term to be very large. - -Note: by default, the integral term in the control system is not accumulated when the vehicle is stationary. This precautionary measure aims to prevent unintended accumulation of the integral term in scenarios where Autoware assumes the vehicle is engaged, but an external system has immobilized the vehicle to initiate startup procedures. - -However, certain situations may arise, such as when the vehicle encounters a depression in the road surface during startup or if the slope compensation is inaccurately estimated (lower than necessary), leading to a failure to initiate motion. To address these scenarios, it is possible to activate error integration even when the vehicle is at rest by setting the `enable_integration_at_low_speed` parameter to true. - -When `enable_integration_at_low_speed` is set to true, the PID controller will initiate integration of the acceleration error after a specified duration defined by the `time_threshold_before_pid_integration` parameter has elapsed without the vehicle surpassing a minimum velocity set by the `current_vel_threshold_pid_integration` parameter. - -The presence of the `time_threshold_before_pid_integration` parameter is important for practical PID tuning. Integrating the error when the vehicle is stationary or at low speed can complicate PID tuning. This parameter effectively introduces a delay before the integral part becomes active, preventing it from kicking in immediately. This delay allows for more controlled and effective tuning of the PID controller. - -At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance. -This may be replaced by a higher performance controller (adaptive control or robust control) in future development. - -#### Time delay compensation - -At high speeds, the delay of actuator systems such as gas pedals and brakes has a significant impact on driving accuracy. -Depending on the actuating principle of the vehicle, the mechanism that physically controls the gas pedal and brake typically has a delay of about a hundred millisecond. - -In this controller, the predicted ego-velocity and the target velocity after the delay time are calculated and used for the feedback to address the time delay problem. - -### Slope compensation - -Based on the slope information, a compensation term is added to the target acceleration. - -There are two sources of the slope information, which can be switched by a parameter. - -- Pitch of the estimated ego-pose (default) - - Calculates the current slope from the pitch angle of the estimated ego-pose - - Pros: Easily available - - Cons: Cannot extract accurate slope information due to the influence of vehicle vibration. -- Z coordinate on the trajectory - - Calculates the road slope from the difference of z-coordinates between the front and rear wheel positions in the target trajectory - - Pros: More accurate than pitch information, if the z-coordinates of the route are properly maintained - - Pros: Can be used in combination with delay compensation (not yet implemented) - - Cons: z-coordinates of high-precision map is needed. - - Cons: Does not support free space planning (for now) - -## Assumptions / Known limits - -1. Smoothed target velocity and its acceleration shall be set in the trajectory - 1. The velocity command is not smoothed inside the controller (only noise may be removed). - 2. For step-like target signal, tracking is performed as fast as possible. -2. The vehicle velocity must be an appropriate value - 1. The ego-velocity must be a signed-value corresponding to the forward/backward direction - 2. The ego-velocity should be given with appropriate noise processing. - 3. If there is a large amount of noise in the ego-velocity, the tracking performance will be significantly reduced. -3. The output of this controller must be achieved by later modules (e.g. vehicle interface). - 1. If the vehicle interface does not have the target velocity or acceleration interface (e.g., the vehicle only has a gas pedal and brake interface), an appropriate conversion must be done after this controller. - -## Inputs / Outputs / API - -### Input - -Set the following from the [controller_node](../trajectory_follower_node/README.md) - -- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. -- `nav_msgs/Odometry`: current odometry - -### Output - -Return LongitudinalOutput which contains the following to the controller node - -- `autoware_control_msgs/Longitudinal`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. -- LongitudinalSyncData - - velocity convergence(currently not used) - -### PIDController class - -The `PIDController` class is straightforward to use. -First, gains and limits must be set (using `setGains()` and `setLimits()`) for the proportional (P), integral (I), and derivative (D) components. -Then, the velocity can be calculated by providing the current error and time step duration to the `calculate()` function. - -## Parameter description - -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 | -| :------------------------------------------ | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| delay_compensation_time | double | delay for longitudinal control [s] | 0.17 | -| enable_smooth_stop | bool | flag to enable transition to STOPPING | true | -| enable_overshoot_emergency | bool | flag to enable transition to EMERGENCY when the ego is over the stop line with a certain distance. See `emergency_state_overshoot_stop_dist`. | true | -| enable_large_tracking_error_emergency | bool | flag to enable transition to EMERGENCY when the closest trajectory point search is failed due to a large deviation between trajectory and ego pose. | true | -| enable_slope_compensation | bool | flag to modify output acceleration for slope compensation. The source of the slope angle can be selected from ego-pose or trajectory angle. See `use_trajectory_for_pitch_calculation`. | true | -| enable_brake_keeping_before_stop | bool | flag to keep a certain acceleration during DRIVE state before the ego stops. See [Brake keeping](#brake-keeping). | false | -| enable_keep_stopped_until_steer_convergence | bool | flag to keep stopped condition until until the steer converges. | true | -| max_acc | double | max value of output acceleration [m/s^2] | 3.0 | -| min_acc | double | min value of output acceleration [m/s^2] | -5.0 | -| max_jerk | double | max value of jerk of output acceleration [m/s^3] | 2.0 | -| min_jerk | double | min value of jerk of output acceleration [m/s^3] | -5.0 | -| use_trajectory_for_pitch_calculation | bool | If true, the slope is estimated from trajectory z-level. Otherwise the pitch angle of the ego pose is used. | false | -| lpf_pitch_gain | double | gain of low-pass filter for pitch estimation | 0.95 | -| max_pitch_rad | double | max value of estimated pitch [rad] | 0.1 | -| min_pitch_rad | double | min value of estimated pitch [rad] | -0.1 | - -### State transition - -| Name | Type | Description | Default value | -| :---------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| drive_state_stop_dist | double | The state will transit to DRIVE when the distance to the stop point is longer than `drive_state_stop_dist` + `drive_state_offset_stop_dist` [m] | 0.5 | -| drive_state_offset_stop_dist | double | The state will transit to DRIVE when the distance to the stop point is longer than `drive_state_stop_dist` + `drive_state_offset_stop_dist` [m] | 1.0 | -| stopping_state_stop_dist | double | The state will transit to STOPPING when the distance to the stop point is shorter than `stopping_state_stop_dist` [m] | 0.5 | -| stopped_state_entry_vel | double | threshold of the ego velocity in transition to the STOPPED state [m/s] | 0.01 | -| stopped_state_entry_acc | double | threshold of the ego acceleration in transition to the STOPPED state [m/s^2] | 0.1 | -| emergency_state_overshoot_stop_dist | double | If `enable_overshoot_emergency` is true and the ego is `emergency_state_overshoot_stop_dist`-meter ahead of the stop point, the state will transit to EMERGENCY. [m] | 1.5 | -| emergency_state_traj_trans_dev | double | If the ego's position is `emergency_state_traj_tran_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m] | 3.0 | -| emergency_state_traj_rot_dev | double | If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad] | 0.784 | - -### DRIVE Parameter - -| Name | Type | Description | Default value | -| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| kp | double | p gain for longitudinal control | 1.0 | -| ki | double | i gain for longitudinal control | 0.1 | -| kd | double | d gain for longitudinal control | 0.0 | -| max_out | double | max value of PID's output acceleration during DRIVE state [m/s^2] | 1.0 | -| min_out | double | min value of PID's output acceleration during DRIVE state [m/s^2] | -1.0 | -| max_p_effort | double | max value of acceleration with p gain | 1.0 | -| min_p_effort | double | min value of acceleration with p gain | -1.0 | -| max_i_effort | double | max value of acceleration with i gain | 0.3 | -| min_i_effort | double | min value of acceleration with i gain | -0.3 | -| max_d_effort | double | max value of acceleration with d gain | 0.0 | -| min_d_effort | double | min value of acceleration with d gain | 0.0 | -| lpf_vel_error_gain | double | gain of low-pass filter for velocity error | 0.9 | -| enable_integration_at_low_speed | bool | Whether to enable integration of acceleration errors when the vehicle speed is lower than `current_vel_threshold_pid_integration` or not. | -| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | -| time_threshold_before_pid_integration | double | How much time without the vehicle moving must past to enable PID error integration. [s] | 5.0 | -| brake_keeping_acc | double | If `enable_brake_keeping_before_stop` is true, a certain acceleration is kept during DRIVE state before the ego stops [m/s^2] See [Brake keeping](#brake-keeping). | 0.2 | - -### STOPPING Parameter (smooth stop) - -Smooth stop is enabled if `enable_smooth_stop` is true. -In smooth stop, strong acceleration (`strong_acc`) will be output first to decrease the ego velocity. -Then weak acceleration (`weak_acc`) will be output to stop smoothly by decreasing the ego jerk. -If the ego does not stop in a certain time or some-meter over the stop point, weak acceleration to stop right (`weak_stop_acc`) now will be output. -If the ego is still running, strong acceleration (`strong_stop_acc`) to stop right now will be output. - -| Name | Type | Description | Default value | -| :--------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| smooth_stop_max_strong_acc | double | max strong acceleration [m/s^2] | -0.5 | -| smooth_stop_min_strong_acc | double | min strong acceleration [m/s^2] | -0.8 | -| smooth_stop_weak_acc | double | weak acceleration [m/s^2] | -0.3 | -| smooth_stop_weak_stop_acc | double | weak acceleration to stop right now [m/s^2] | -0.8 | -| smooth_stop_strong_stop_acc | double | strong acceleration to be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m/s^2] | -3.4 | -| smooth_stop_max_fast_vel | double | max fast vel to judge the ego is running fast [m/s]. If the ego is running fast, strong acceleration will be output. | 0.5 | -| smooth_stop_min_running_vel | double | min ego velocity to judge if the ego is running or not [m/s] | 0.01 | -| smooth_stop_min_running_acc | double | min ego acceleration to judge if the ego is running or not [m/s^2] | 0.01 | -| smooth_stop_weak_stop_time | double | max time to output weak acceleration [s]. After this, strong acceleration will be output. | 0.8 | -| smooth_stop_weak_stop_dist | double | Weak acceleration will be output when the ego is `smooth_stop_weak_stop_dist`-meter before the stop point. [m] | -0.3 | -| smooth_stop_strong_stop_dist | double | Strong acceleration will be output when the ego is `smooth_stop_strong_stop_dist`-meter over the stop point. [m] | -0.5 | - -### STOPPED Parameter - -The `STOPPED` state assumes that the vehicle is completely stopped with the brakes fully applied. -Therefore, `stopped_acc` should be set to a value that allows the vehicle to apply the strongest possible brake. -If `stopped_acc` is not sufficiently low, there is a possibility of sliding down on steep slopes. - -| Name | Type | Description | Default value | -| :----------- | :----- | :------------------------------------------- | :------------ | -| stopped_vel | double | target velocity in STOPPED state [m/s] | 0.0 | -| stopped_acc | double | target acceleration in STOPPED state [m/s^2] | -3.4 | -| stopped_jerk | double | target jerk in STOPPED state [m/s^3] | -5.0 | - -### EMERGENCY Parameter - -| Name | Type | Description | Default value | -| :------------- | :----- | :------------------------------------------------ | :------------ | -| emergency_vel | double | target velocity in EMERGENCY state [m/s] | 0.0 | -| emergency_acc | double | target acceleration in an EMERGENCY state [m/s^2] | -5.0 | -| emergency_jerk | double | target jerk in an EMERGENCY state [m/s^3] | -3.0 | - -## References / External links - -## Future extensions / Unimplemented parts - -## Related issues diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp deleted file mode 100644 index 06fe1793c8123..0000000000000 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2018-2021 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 PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ - -#include -#include -#include -#include - -namespace autoware::motion::control::pid_longitudinal_controller -{ - -/** - * @brief Simple filter with gain on the first derivative of the value - */ -class LowpassFilter1d -{ -private: - double m_x; //!< @brief current filtered value - double m_gain; //!< @brief gain value of first-order low-pass filter - -public: - /** - * @brief constructor with initial value and first-order gain - * @param [in] x initial value - * @param [in] gain first-order gain - */ - LowpassFilter1d(const double x, const double gain) : m_x(x), m_gain(gain) {} - - /** - * @brief set the current value of the filter - * @param [in] x new value - */ - void reset(const double x) { m_x = x; } - - /** - * @brief get the current value of the filter - */ - double getValue() const { return m_x; } - - /** - * @brief filter a new value - * @param [in] u new value - */ - double filter(const double u) - { - const double ret = m_gain * m_x + (1.0 - m_gain) * u; - m_x = ret; - return ret; - } -}; -} // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp deleted file mode 100644 index 8b981c0485ed9..0000000000000 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright 2018-2021 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 PID_LONGITUDINAL_CONTROLLER__PID_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__PID_HPP_ - -#include - -namespace autoware::motion::control::pid_longitudinal_controller -{ - -/// @brief implementation of a PID controller -class PIDController -{ -public: - PIDController(); - - /** - * @brief calculate the output of this PID - * @param [in] error previous error - * @param [in] dt time step [s] - * @param [in] is_integrated if true, will use the integral component for calculation - * @param [out] pid_contributions values of the proportional, integral, and derivative components - * @return PID output - * @throw std::runtime_error if gains or limits have not been set - */ - double calculate( - const double error, const double dt, const bool is_integrated, - std::vector & pid_contributions); - /** - * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms - * @param [in] kp proportional coefficient - * @param [in] ki integral coefficient - * @param [in] kd derivative coefficient - */ - void setGains(const double kp, const double ki, const double kd); - /** - * @brief set limits on the total, proportional, integral, and derivative components - * @param [in] max_ret maximum return value of this PID - * @param [in] min_ret minimum return value of this PID - * @param [in] max_ret_p maximum value of the proportional component - * @param [in] min_ret_p minimum value of the proportional component - * @param [in] max_ret_i maximum value of the integral component - * @param [in] min_ret_i minimum value of the integral component - * @param [in] max_ret_d maximum value of the derivative component - * @param [in] min_ret_d minimum value of the derivative component - */ - void setLimits( - const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, - const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d); - /** - * @brief reset this PID to its initial state - */ - void reset(); - -private: - // PID parameters - struct Params - { - double kp; - double ki; - double kd; - double max_ret_p; - double min_ret_p; - double max_ret_i; - double min_ret_i; - double max_ret_d; - double min_ret_d; - double max_ret; - double min_ret; - }; - Params m_params; - - // state variables - double m_error_integral; - double m_prev_error; - bool m_is_first_time; - bool m_is_gains_set; - bool m_is_limits_set; -}; -} // namespace autoware::motion::control::pid_longitudinal_controller - -#endif // PID_LONGITUDINAL_CONTROLLER__PID_HPP_ diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml deleted file mode 100644 index 82b297a69fee6..0000000000000 --- a/control/pid_longitudinal_controller/package.xml +++ /dev/null @@ -1,49 +0,0 @@ - - - - pid_longitudinal_controller - 1.0.0 - PID-based longitudinal controller - - Takamasa Horibe - Takayuki Murooka - Mamoru Sobue - - Apache 2.0 - - Takamasa Horibe - Maxime CLEMENT - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - autoware_adapi_v1_msgs - autoware_control_msgs - autoware_planning_msgs - autoware_vehicle_msgs - diagnostic_msgs - diagnostic_updater - eigen - geometry_msgs - interpolation - motion_utils - rclcpp - rclcpp_components - std_msgs - tf2 - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - trajectory_follower_base - vehicle_info_util - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/control/pid_longitudinal_controller/src/pid.cpp b/control/pid_longitudinal_controller/src/pid.cpp deleted file mode 100644 index 530b5cd15e754..0000000000000 --- a/control/pid_longitudinal_controller/src/pid.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// Copyright 2018-2021 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 "pid_longitudinal_controller/pid.hpp" - -#include -#include -#include -#include -#include - -namespace autoware::motion::control::pid_longitudinal_controller -{ -PIDController::PIDController() -: m_error_integral(0.0), - m_prev_error(0.0), - m_is_first_time(true), - m_is_gains_set(false), - m_is_limits_set(false) -{ -} - -double PIDController::calculate( - const double error, const double dt, const bool enable_integration, - std::vector & pid_contributions) -{ - if (!m_is_gains_set || !m_is_limits_set) { - throw std::runtime_error("Trying to calculate uninitialized PID"); - } - - const auto & p = m_params; - - double ret_p = p.kp * error; - ret_p = std::min(std::max(ret_p, p.min_ret_p), p.max_ret_p); - - if (enable_integration) { - m_error_integral += error * dt; - m_error_integral = std::min(std::max(m_error_integral, p.min_ret_i / p.ki), p.max_ret_i / p.ki); - } - const double ret_i = p.ki * m_error_integral; - - double error_differential; - if (m_is_first_time) { - error_differential = 0; - m_is_first_time = false; - } else { - error_differential = (error - m_prev_error) / dt; - } - double ret_d = p.kd * error_differential; - ret_d = std::min(std::max(ret_d, p.min_ret_d), p.max_ret_d); - - m_prev_error = error; - - pid_contributions.resize(3); - pid_contributions.at(0) = ret_p; - pid_contributions.at(1) = ret_i; - pid_contributions.at(2) = ret_d; - - double ret = ret_p + ret_i + ret_d; - ret = std::min(std::max(ret, p.min_ret), p.max_ret); - - return ret; -} - -void PIDController::setGains(const double kp, const double ki, const double kd) -{ - m_params.kp = kp; - m_params.ki = ki; - m_params.kd = kd; - m_is_gains_set = true; -} - -void PIDController::setLimits( - const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p, - const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d) -{ - m_params.max_ret = max_ret; - m_params.min_ret = min_ret; - m_params.max_ret_p = max_ret_p; - m_params.min_ret_p = min_ret_p; - m_params.max_ret_d = max_ret_d; - m_params.min_ret_d = min_ret_d; - m_params.max_ret_i = max_ret_i; - m_params.min_ret_i = min_ret_i; - m_is_limits_set = true; -} - -void PIDController::reset() -{ - m_error_integral = 0.0; - m_prev_error = 0.0; - m_is_first_time = true; -} -} // namespace autoware::motion::control::pid_longitudinal_controller 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 index 8410e7c78f723..2a7a552c1973d 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -15,6 +15,7 @@ #ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ #define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#include #include #include #include @@ -23,7 +24,6 @@ #include #include #include -#include #include #include @@ -120,7 +120,7 @@ class CollisionChecker // Variables std::shared_ptr debug_ptr_; rclcpp::Node * node_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::vector predicted_object_history_{}; }; } // namespace autoware::motion::control::predicted_path_checker 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 index 23696887c7051..6c4832a0dac3e 100644 --- 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 @@ -15,6 +15,8 @@ #ifndef PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ #define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#include +#include #include #include #include @@ -24,8 +26,6 @@ #include #include #include -#include -#include #include #include @@ -116,7 +116,7 @@ class PredictedPathCheckerNode : public rclcpp::Node // Variables State current_state_{State::DRIVE}; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; bool is_calling_set_stop_{false}; bool is_stopped_by_node_{false}; diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 957800ad04305..d9299f09dfb6b 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -15,10 +15,10 @@ #ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_ #define PREDICTED_PATH_CHECKER__UTILS_HPP_ +#include #include #include #include -#include #include #include @@ -39,6 +39,7 @@ namespace utils { +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -48,7 +49,6 @@ 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; @@ -57,7 +57,7 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::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); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width); TrajectoryPoint calcInterpolatedPoint( const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, @@ -65,7 +65,7 @@ TrajectoryPoint calcInterpolatedPoint( std::pair findStopPoint( TrajectoryPoints & predicted_trajectory_array, const size_t collision_idx, - const double stop_margin, vehicle_info_util::VehicleInfo & vehicle_info); + const double stop_margin, autoware::vehicle_info_utils::VehicleInfo & vehicle_info); bool isInBrakeDistance( const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, diff --git a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml index a35c11b80c1f7..6af1372a5bb4a 100755 --- a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml +++ b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index bca65302dba55..911344c578414 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -14,6 +14,7 @@ autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils boost component_interface_specs component_interface_utils @@ -34,7 +35,6 @@ tier4_control_msgs tier4_external_api_msgs tier4_planning_msgs - vehicle_info_util ament_cmake_ros ament_lint_auto 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 index 5e8d96805b832..7d392571546b4 100644 --- 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 @@ -27,7 +27,7 @@ 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()) + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo()) { } 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 index 28ea21f639a0e..38717a1849ad9 100644 --- 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 @@ -38,7 +38,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n 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(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); // Node Parameter node_param_.update_rate = declare_parameter("update_rate", 10.0); 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 index f1c76ce6eef8f..a9c3e2b24f9a5 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -29,7 +29,7 @@ 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) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) { Polygon2d polygon; @@ -146,7 +146,7 @@ TrajectoryPoint calcInterpolatedPoint( std::pair findStopPoint( TrajectoryPoints & trajectory_array, const size_t collision_idx, const double stop_margin, - vehicle_info_util::VehicleInfo & vehicle_info) + autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { // It returns the stop point and segment of the point on trajectory. diff --git a/control/pure_pursuit/CMakeLists.txt b/control/pure_pursuit/CMakeLists.txt deleted file mode 100644 index b1c73d5397f07..0000000000000 --- a/control/pure_pursuit/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(pure_pursuit) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) - -include_directories( - SYSTEM - ${EIGEN3_INCLUDE_DIRS} -) - -# pure_pursuit_core -ament_auto_add_library(pure_pursuit_core SHARED - src/pure_pursuit_core/planning_utils.cpp - src/pure_pursuit_core/pure_pursuit.cpp - src/pure_pursuit_core/interpolate.cpp -) - -# pure_pursuit -ament_auto_add_library(pure_pursuit_lateral_controller SHARED - src/pure_pursuit/pure_pursuit_lateral_controller.cpp - src/pure_pursuit/pure_pursuit_viz.cpp -) - -target_link_libraries(pure_pursuit_lateral_controller - pure_pursuit_core -) - -rclcpp_components_register_node(pure_pursuit_lateral_controller - PLUGIN "pure_pursuit::PurePursuitLateralController" - EXECUTABLE pure_pursuit_lateral_controller_exe -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/control/pure_pursuit/README.md b/control/pure_pursuit/README.md deleted file mode 100644 index 55c847fe88d22..0000000000000 --- a/control/pure_pursuit/README.md +++ /dev/null @@ -1,19 +0,0 @@ -# Pure Pursuit Controller - -The Pure Pursuit Controller module calculates the steering angle for tracking a desired trajectory using the pure pursuit algorithm. This is used as a lateral controller plugin in the `trajectory_follower_node`. - -## Inputs - -Set the following from the [controller_node](../trajectory_follower_node/README.md) - -- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. -- `nav_msgs/Odometry`: current ego pose and velocity information - -## Outputs - -Return LateralOutput which contains the following to the controller node - -- `autoware_control_msgs/Lateral`: target steering angle -- LateralSyncData - - steer angle convergence -- `autoware_planning_msgs/Trajectory`: predicted path for ego vehicle diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp deleted file mode 100644 index 99b1a82ab2dce..0000000000000 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit.hpp +++ /dev/null @@ -1,88 +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. -/* - * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_HPP_ -#define PURE_PURSUIT__PURE_PURSUIT_HPP_ - -#include - -#include - -#include -#include -#include - -#define EIGEN_MPL2_ONLY -#include -#include - -namespace pure_pursuit -{ -class PurePursuit -{ -public: - PurePursuit() : lookahead_distance_(0.0), closest_thr_dist_(3.0), closest_thr_ang_(M_PI / 4) {} - ~PurePursuit() = default; - - rclcpp::Logger logger = rclcpp::get_logger("pure_pursuit"); - // setter - void setCurrentPose(const geometry_msgs::msg::Pose & msg); - void setWaypoints(const std::vector & msg); - void setLookaheadDistance(double ld) { lookahead_distance_ = ld; } - void setClosestThreshold(double closest_thr_dist, double closest_thr_ang) - { - closest_thr_dist_ = closest_thr_dist; - closest_thr_ang_ = closest_thr_ang; - } - - // getter - geometry_msgs::msg::Point getLocationOfNextWaypoint() const { return loc_next_wp_; } - geometry_msgs::msg::Point getLocationOfNextTarget() const { return loc_next_tgt_; } - - bool isDataReady(); - std::pair run(); // calculate curvature - -private: - // variables for debug - geometry_msgs::msg::Point loc_next_wp_; - geometry_msgs::msg::Point loc_next_tgt_; - - // variables got from outside - double lookahead_distance_, closest_thr_dist_, closest_thr_ang_; - std::shared_ptr> curr_wps_ptr_; - std::shared_ptr curr_pose_ptr_; - - // functions - int32_t findNextPointIdx(int32_t search_start_idx); - std::pair lerpNextTarget(int32_t next_wp_idx); -}; - -} // namespace pure_pursuit - -#endif // PURE_PURSUIT__PURE_PURSUIT_HPP_ 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 deleted file mode 100644 index 5b7b466dcb4dd..0000000000000 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2020-2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. -// -// 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 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ -#define PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ - -#include "pure_pursuit/pure_pursuit.hpp" -#include "pure_pursuit/pure_pursuit_viz.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" -#include "trajectory_follower_base/lateral_controller_base.hpp" - -#include -#include -#include - -#include "autoware_control_msgs/msg/lateral.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" - -#include // To be replaced by std::optional in C++17 - -#include -#include - -using autoware::motion::control::trajectory_follower::InputData; -using autoware::motion::control::trajectory_follower::LateralControllerBase; -using autoware::motion::control::trajectory_follower::LateralOutput; -using autoware_control_msgs::msg::Lateral; -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; - -namespace pure_pursuit -{ - -struct PpOutput -{ - double curvature; - double velocity; -}; - -struct Param -{ - // Global Parameters - double wheel_base; - double max_steering_angle; // [rad] - - // Algorithm Parameters - double ld_velocity_ratio; - double ld_lateral_error_ratio; - double ld_curvature_ratio; - double min_lookahead_distance; - double max_lookahead_distance; - double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear - double converged_steer_rad_; - double prediction_ds; - double prediction_distance_length; // Total distance of prediction trajectory - double resampling_ds; - double curvature_calculation_distance; - double long_ld_lateral_error_threshold; - bool enable_path_smoothing; - int path_filter_moving_ave_num; -}; - -struct DebugData -{ - geometry_msgs::msg::Point next_target; -}; - -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::Clock::SharedPtr clock_; - rclcpp::Logger logger_; - std::vector output_tp_array_; - autoware_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; - autoware_planning_msgs::msg::Trajectory trajectory_; - nav_msgs::msg::Odometry current_odometry_; - autoware_vehicle_msgs::msg::SteeringReport current_steering_; - boost::optional prev_cmd_; - - // Debug Publisher - rclcpp::Publisher::SharedPtr pub_debug_marker_; - rclcpp::Publisher::SharedPtr pub_debug_values_; - // Predicted Trajectory publish - rclcpp::Publisher::SharedPtr pub_predicted_trajectory_; - - void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - - void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - - void setResampledTrajectory(); - - // TF - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - geometry_msgs::msg::Pose current_pose_; - - void publishDebugMarker() const; - - /** - * @brief compute control command for path follow with a constant control period - */ - bool isReady([[maybe_unused]] const InputData & input_data) override; - LateralOutput run(const InputData & input_data) override; - - Lateral generateCtrlCmdMsg(const double target_curvature); - - // Parameter - Param param_{}; - - // Algorithm - std::unique_ptr pure_pursuit_; - - boost::optional calcTargetCurvature( - bool is_control_output, geometry_msgs::msg::Pose pose); - - /** - * @brief It takes current pose, control command, and delta distance. Then it calculates next pose - * of vehicle. - */ - - TrajectoryPoint calcNextPose(const double ds, TrajectoryPoint & point, Lateral cmd) const; - - boost::optional generatePredictedTrajectory(); - - Lateral generateOutputControlCmd(); - - bool calcIsSteerConverged(const Lateral & cmd); - - double calcLookaheadDistance( - const double lateral_error, const double curvature, const double velocity, const double min_ld, - const bool is_control_cmd); - - double calcCurvature(const size_t closest_idx); - - void averageFilterTrajectory(autoware_planning_msgs::msg::Trajectory & u); - - // Debug - mutable DebugData debug_data_; -}; - -} // namespace pure_pursuit - -#endif // PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp deleted file mode 100644 index a5ae31133be8e..0000000000000 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp +++ /dev/null @@ -1,127 +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. -/* - * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_ -#define PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_ - -#include "pure_pursuit/pure_pursuit.hpp" -#include "pure_pursuit/pure_pursuit_viz.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -#include // To be replaced by std::optional in C++17 - -#include -#include - -#include -#include - -namespace pure_pursuit -{ -struct Param -{ - // Global Parameters - double wheel_base; - - // Node Parameters - double ctrl_period; - - // Algorithm Parameters - double lookahead_distance_ratio; - double min_lookahead_distance; - double reverse_min_lookahead_distance; // min_lookahead_distance in reverse gear -}; - -struct DebugData -{ - geometry_msgs::msg::Point next_target; -}; - -class PurePursuitNode : public rclcpp::Node -{ -public: - explicit PurePursuitNode(const rclcpp::NodeOptions & node_options); - -private: - // Subscriber - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Subscription::SharedPtr sub_current_odometry_; - - autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; - nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; - - bool isDataReady(); - - void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - - // TF - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - - // Publisher - rclcpp::Publisher::SharedPtr pub_ctrl_cmd_; - - void publishCommand(const double target_curvature); - - // Debug Publisher - rclcpp::Publisher::SharedPtr pub_debug_marker_; - - void publishDebugMarker() const; - - // Timer - rclcpp::TimerBase::SharedPtr timer_; - void onTimer(); - - // Parameter - Param param_; - - // Algorithm - std::unique_ptr pure_pursuit_; - - boost::optional calcTargetCurvature(); - boost::optional calcTargetPoint() const; - - // Debug - mutable DebugData debug_data_; -}; - -} // namespace pure_pursuit - -#endif // PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_viz.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_viz.hpp deleted file mode 100644 index adc803538703f..0000000000000 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_viz.hpp +++ /dev/null @@ -1,47 +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. -/* - * Copyright 2015-2019 Autoware Foundation. 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 PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_ -#define PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_ - -#include -#include -#include - -#include -namespace pure_pursuit -{ -visualization_msgs::msg::Marker createNextTargetMarker( - const geometry_msgs::msg::Point & next_target); - -visualization_msgs::msg::Marker createTrajectoryCircleMarker( - const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose); -} // namespace pure_pursuit - -#endif // PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_ diff --git a/control/pure_pursuit/include/pure_pursuit/util/marker_helper.hpp b/control/pure_pursuit/include/pure_pursuit/util/marker_helper.hpp deleted file mode 100644 index 56943b4072fb3..0000000000000 --- a/control/pure_pursuit/include/pure_pursuit/util/marker_helper.hpp +++ /dev/null @@ -1,107 +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 PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ -#define PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ - -#include - -#include - -#include - -namespace pure_pursuit -{ -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 createDefaultMarker( - const std::string & frame_id, 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; - // ToDo - // marker.header.stamp = rclcpp::Node::now(); - marker.ns = ns; - marker.id = id; - marker.type = type; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration::from_seconds(0); - - 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; -} - -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 pure_pursuit - -#endif // PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_ diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml deleted file mode 100644 index b4820aee5cec4..0000000000000 --- a/control/pure_pursuit/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - pure_pursuit - 0.1.0 - The pure_pursuit package - Takamasa Horibe - Apache License 2.0 - - Berkay Karaman - Takamasa Horibe - - ament_cmake_auto - autoware_cmake - - autoware_control_msgs - autoware_planning_msgs - boost - geometry_msgs - libboost-dev - motion_utils - nav_msgs - rclcpp - rclcpp_components - sensor_msgs - std_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - trajectory_follower_base - vehicle_info_util - visualization_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - 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 deleted file mode 100644 index a8232cce5d08d..0000000000000 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ /dev/null @@ -1,490 +0,0 @@ -// Copyright 2020-2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. -// -// 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 2015-2019 Autoware Foundation. 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 "pure_pursuit/pure_pursuit_lateral_controller.hpp" - -#include "pure_pursuit/pure_pursuit_viz.hpp" -#include "pure_pursuit/util/planning_utils.hpp" -#include "pure_pursuit/util/tf_utils.hpp" - -#include - -#include -#include -#include - -namespace -{ -enum TYPE { - VEL_LD = 0, - CURVATURE_LD = 1, - LATERAL_ERROR_LD = 2, - TOTAL_LD = 3, - CURVATURE = 4, - LATERAL_ERROR = 5, - VELOCITY = 6, - SIZE // this is the number of enum elements -}; -} // namespace - -namespace pure_pursuit -{ -PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) -: 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(); - 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_.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"); - 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"); - 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"); - - // Debug Publishers - pub_debug_marker_ = - 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( - "~/output/predicted_trajectory", 1); -} - -double PurePursuitLateralController::calcLookaheadDistance( - const double lateral_error, const double curvature, const double velocity, const double min_ld, - const bool is_control_cmd) -{ - const double vel_ld = abs(param_.ld_velocity_ratio * velocity); - const double curvature_ld = -abs(param_.ld_curvature_ratio * curvature); - double lateral_error_ld = 0.0; - - if (abs(lateral_error) >= param_.long_ld_lateral_error_threshold) { - // If lateral error is higher than threshold, we should make ld larger to prevent entering the - // road with high heading error. - lateral_error_ld = abs(param_.ld_lateral_error_ratio * lateral_error); - } - - const double total_ld = - std::clamp(vel_ld + curvature_ld + lateral_error_ld, min_ld, param_.max_lookahead_distance); - - auto pubDebugValues = [&]() { - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; - debug_msg.data.resize(TYPE::SIZE); - debug_msg.data.at(TYPE::VEL_LD) = static_cast(vel_ld); - debug_msg.data.at(TYPE::CURVATURE_LD) = static_cast(curvature_ld); - debug_msg.data.at(TYPE::LATERAL_ERROR_LD) = static_cast(lateral_error_ld); - debug_msg.data.at(TYPE::TOTAL_LD) = static_cast(total_ld); - 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 = clock_->now(); - pub_debug_values_->publish(debug_msg); - }; - - if (is_control_cmd) { - pubDebugValues(); - } - - return total_ld; -} - -TrajectoryPoint PurePursuitLateralController::calcNextPose( - const double ds, TrajectoryPoint & point, Lateral cmd) const -{ - geometry_msgs::msg::Transform transform; - transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); - transform.rotation = - planning_utils::getQuaternionFromYaw(((tan(cmd.steering_tire_angle) * ds) / param_.wheel_base)); - TrajectoryPoint output_p; - - tf2::Transform tf_pose; - tf2::Transform tf_offset; - tf2::fromMsg(transform, tf_offset); - tf2::fromMsg(point.pose, tf_pose); - tf2::toMsg(tf_pose * tf_offset, output_p.pose); - return output_p; -} - -void PurePursuitLateralController::setResampledTrajectory() -{ - // Interpolate with constant interval distance. - std::vector out_arclength; - const auto input_tp_array = motion_utils::convertToTrajectoryPointArray(trajectory_); - const auto traj_length = motion_utils::calcArcLength(input_tp_array); - for (double s = 0; s < traj_length; s += param_.resampling_ds) { - out_arclength.push_back(s); - } - trajectory_resampled_ = - std::make_shared(motion_utils::resampleTrajectory( - motion_utils::convertToTrajectory(input_tp_array), out_arclength)); - trajectory_resampled_->points.back() = trajectory_.points.back(); - trajectory_resampled_->header = trajectory_.header; - output_tp_array_ = motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_); -} - -double PurePursuitLateralController::calcCurvature(const size_t closest_idx) -{ - // Calculate current curvature - const size_t idx_dist = static_cast( - std::max(static_cast((param_.curvature_calculation_distance) / param_.resampling_ds), 1)); - - // Find the points in trajectory to calculate curvature - size_t next_idx = trajectory_resampled_->points.size() - 1; - size_t prev_idx = 0; - - if (static_cast(closest_idx) >= idx_dist) { - prev_idx = closest_idx - idx_dist; - } else { - // return zero curvature when backward distance is not long enough in the trajectory - return 0.0; - } - - if (trajectory_resampled_->points.size() - 1 >= closest_idx + idx_dist) { - next_idx = closest_idx + idx_dist; - } else { - // return zero curvature when forward distance is not long enough in the trajectory - return 0.0; - } - // TODO(k.sugahara): shift the center point of the curvature calculation to allow sufficient - // distance, because if sufficient distance cannot be obtained in front or behind, the curvature - // will be zero in the current implementation. - - // Calculate curvature assuming the trajectory points interval is constant - double current_curvature = 0.0; - - try { - current_curvature = tier4_autoware_utils::calcCurvature( - tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(prev_idx)), - tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(closest_idx)), - tier4_autoware_utils::getPoint(trajectory_resampled_->points.at(next_idx))); - } catch (std::exception const & e) { - // ...code that handles the error... - RCLCPP_WARN(rclcpp::get_logger("pure_pursuit"), "%s", e.what()); - current_curvature = 0.0; - } - return current_curvature; -} - -void PurePursuitLateralController::averageFilterTrajectory( - autoware_planning_msgs::msg::Trajectory & u) -{ - if (static_cast(u.points.size()) <= 2 * param_.path_filter_moving_ave_num) { - RCLCPP_ERROR(logger_, "Cannot smooth path! Trajectory size is too low!"); - return; - } - - autoware_planning_msgs::msg::Trajectory filtered_trajectory(u); - - for (int64_t i = 0; i < static_cast(u.points.size()); ++i) { - TrajectoryPoint tmp{}; - int64_t num_tmp = param_.path_filter_moving_ave_num; - int64_t count = 0; - double yaw = 0.0; - if (i - num_tmp < 0) { - num_tmp = i; - } - if (i + num_tmp > static_cast(u.points.size()) - 1) { - num_tmp = static_cast(u.points.size()) - i - 1; - } - for (int64_t j = -num_tmp; j <= num_tmp; ++j) { - const auto & p = u.points.at(static_cast(i + j)); - - tmp.pose.position.x += p.pose.position.x; - tmp.pose.position.y += p.pose.position.y; - tmp.pose.position.z += p.pose.position.z; - tmp.longitudinal_velocity_mps += p.longitudinal_velocity_mps; - tmp.acceleration_mps2 += p.acceleration_mps2; - tmp.front_wheel_angle_rad += p.front_wheel_angle_rad; - tmp.heading_rate_rps += p.heading_rate_rps; - yaw += tf2::getYaw(p.pose.orientation); - tmp.lateral_velocity_mps += p.lateral_velocity_mps; - tmp.rear_wheel_angle_rad += p.rear_wheel_angle_rad; - ++count; - } - auto & p = filtered_trajectory.points.at(static_cast(i)); - - p.pose.position.x = tmp.pose.position.x / count; - p.pose.position.y = tmp.pose.position.y / count; - p.pose.position.z = tmp.pose.position.z / count; - p.longitudinal_velocity_mps = tmp.longitudinal_velocity_mps / count; - p.acceleration_mps2 = tmp.acceleration_mps2 / count; - p.front_wheel_angle_rad = tmp.front_wheel_angle_rad / count; - p.heading_rate_rps = tmp.heading_rate_rps / count; - p.lateral_velocity_mps = tmp.lateral_velocity_mps / count; - p.rear_wheel_angle_rad = tmp.rear_wheel_angle_rad / count; - p.pose.orientation = pure_pursuit::planning_utils::getQuaternionFromYaw(yaw / count); - } - trajectory_resampled_ = std::make_shared(filtered_trajectory); -} - -boost::optional PurePursuitLateralController::generatePredictedTrajectory() -{ - const auto closest_idx_result = - motion_utils::findNearestIndex(output_tp_array_, current_odometry_.pose.pose, 3.0, M_PI_4); - - if (!closest_idx_result) { - return boost::none; - } - - const double remaining_distance = planning_utils::calcArcLengthFromWayPoint( - *trajectory_resampled_, *closest_idx_result, trajectory_resampled_->points.size() - 1); - - const auto num_of_iteration = std::max( - static_cast(std::ceil( - std::min(remaining_distance, param_.prediction_distance_length) / param_.prediction_ds)), - 1); - Trajectory predicted_trajectory; - - // Iterative prediction: - for (int i = 0; i < num_of_iteration; i++) { - if (i == 0) { - // For first point, use the odometry for velocity, and use the current_pose for prediction. - - TrajectoryPoint p; - p.pose = current_odometry_.pose.pose; - p.longitudinal_velocity_mps = current_odometry_.twist.twist.linear.x; - predicted_trajectory.points.push_back(p); - - const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose); - Lateral tmp_msg; - - if (pp_output) { - tmp_msg = generateCtrlCmdMsg(pp_output->curvature); - predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; - } else { - RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); - tmp_msg = generateCtrlCmdMsg(0.0); - } - TrajectoryPoint p2; - p2 = calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg); - predicted_trajectory.points.push_back(p2); - - } else { - const auto pp_output = calcTargetCurvature(false, predicted_trajectory.points.at(i).pose); - Lateral tmp_msg; - - if (pp_output) { - tmp_msg = generateCtrlCmdMsg(pp_output->curvature); - predicted_trajectory.points.at(i).longitudinal_velocity_mps = pp_output->velocity; - } else { - RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "failed to solve pure_pursuit for prediction"); - tmp_msg = generateCtrlCmdMsg(0.0); - } - predicted_trajectory.points.push_back( - calcNextPose(param_.prediction_ds, predicted_trajectory.points.at(i), tmp_msg)); - } - } - - // for last point - predicted_trajectory.points.back().longitudinal_velocity_mps = 0.0; - predicted_trajectory.header.frame_id = trajectory_resampled_->header.frame_id; - predicted_trajectory.header.stamp = trajectory_resampled_->header.stamp; - - return predicted_trajectory; -} - -bool PurePursuitLateralController::isReady([[maybe_unused]] const InputData & input_data) -{ - return true; -} - -LateralOutput PurePursuitLateralController::run(const InputData & input_data) -{ - current_pose_ = input_data.current_odometry.pose.pose; - trajectory_ = input_data.current_trajectory; - current_odometry_ = input_data.current_odometry; - current_steering_ = input_data.current_steering; - - setResampledTrajectory(); - if (param_.enable_path_smoothing) { - averageFilterTrajectory(*trajectory_resampled_); - } - const auto cmd_msg = generateOutputControlCmd(); - - LateralOutput output; - output.control_cmd = cmd_msg; - output.sync_data.is_steer_converged = calcIsSteerConverged(cmd_msg); - - // calculate predicted trajectory with iterative calculation - const auto predicted_trajectory = generatePredictedTrajectory(); - if (!predicted_trajectory) { - RCLCPP_ERROR(logger_, "Failed to generate predicted trajectory."); - } else { - pub_predicted_trajectory_->publish(*predicted_trajectory); - } - - return output; -} - -bool PurePursuitLateralController::calcIsSteerConverged(const Lateral & cmd) -{ - return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) < - static_cast(param_.converged_steer_rad_); -} - -Lateral PurePursuitLateralController::generateOutputControlCmd() -{ - // Generate the control command - const auto pp_output = calcTargetCurvature(true, current_odometry_.pose.pose); - Lateral output_cmd; - - if (pp_output) { - output_cmd = generateCtrlCmdMsg(pp_output->curvature); - prev_cmd_ = boost::optional(output_cmd); - publishDebugMarker(); - } else { - RCLCPP_WARN_THROTTLE( - logger_, *clock_, 5000, "failed to solve pure_pursuit for control command calculation"); - if (prev_cmd_) { - output_cmd = *prev_cmd_; - } else { - output_cmd = generateCtrlCmdMsg(0.0); - } - } - return output_cmd; -} - -Lateral PurePursuitLateralController::generateCtrlCmdMsg(const double target_curvature) -{ - const double tmp_steering = - planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); - Lateral cmd; - cmd.stamp = clock_->now(); - cmd.steering_tire_angle = static_cast( - std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle)); - - // pub_ctrl_cmd_->publish(cmd); - return cmd; -} - -void PurePursuitLateralController::publishDebugMarker() const -{ - visualization_msgs::msg::MarkerArray marker_array; - - marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); - marker_array.markers.push_back( - createTrajectoryCircleMarker(debug_data_.next_target, current_odometry_.pose.pose)); -} - -boost::optional PurePursuitLateralController::calcTargetCurvature( - bool is_control_output, geometry_msgs::msg::Pose pose) -{ - // Ignore invalid trajectory - if (trajectory_resampled_->points.size() < 3) { - RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "received path size is < 3, ignored"); - return {}; - } - - // Calculate target point for velocity/acceleration - - const auto closest_idx_result = - motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4); - if (!closest_idx_result) { - RCLCPP_ERROR(logger_, "cannot find closest waypoint"); - return {}; - } - - const double target_vel = - trajectory_resampled_->points.at(*closest_idx_result).longitudinal_velocity_mps; - - // calculate the lateral error - - const double lateral_error = - motion_utils::calcLateralOffset(trajectory_resampled_->points, pose.position); - - // calculate the current curvature - - const double current_curvature = calcCurvature(*closest_idx_result); - - // Calculate lookahead distance - - const bool is_reverse = (target_vel < 0); - const double min_lookahead_distance = - is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; - double lookahead_distance = min_lookahead_distance; - if (is_control_output) { - lookahead_distance = calcLookaheadDistance( - lateral_error, current_curvature, current_odometry_.twist.twist.linear.x, - min_lookahead_distance, is_control_output); - } else { - lookahead_distance = calcLookaheadDistance( - lateral_error, current_curvature, target_vel, min_lookahead_distance, is_control_output); - } - - // Set PurePursuit data - pure_pursuit_->setCurrentPose(pose); - pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_resampled_)); - pure_pursuit_->setLookaheadDistance(lookahead_distance); - - // Run PurePursuit - const auto pure_pursuit_result = pure_pursuit_->run(); - if (!pure_pursuit_result.first) { - return {}; - } - - const auto kappa = pure_pursuit_result.second; - - // Set debug data - if (is_control_output) { - debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); - } - PpOutput output{}; - output.curvature = kappa; - if (!is_control_output) { - output.velocity = current_odometry_.twist.twist.linear.x; - } else { - output.velocity = target_vel; - } - - return output; -} -} // namespace pure_pursuit diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp deleted file mode 100644 index a4b491930df1e..0000000000000 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp +++ /dev/null @@ -1,230 +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. - -/* - * Copyright 2015-2019 Autoware Foundation. 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 "pure_pursuit/pure_pursuit_node.hpp" - -#include "pure_pursuit/pure_pursuit_viz.hpp" -#include "pure_pursuit/util/planning_utils.hpp" -#include "pure_pursuit/util/tf_utils.hpp" - -#include - -#include -#include -#include - -namespace -{ -double calcLookaheadDistance( - const double velocity, const double lookahead_distance_ratio, const double min_lookahead_distance) -{ - const double lookahead_distance = lookahead_distance_ratio * std::abs(velocity); - return std::max(lookahead_distance, min_lookahead_distance); -} - -} // namespace - -namespace pure_pursuit -{ -PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) -: Node("pure_pursuit", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) -{ - pure_pursuit_ = std::make_unique(); - - // Vehicle Parameters - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); - param_.wheel_base = vehicle_info.wheel_base_m; - - // Node Parameters - param_.ctrl_period = this->declare_parameter("control_period"); - - // Algorithm Parameters - param_.lookahead_distance_ratio = this->declare_parameter("lookahead_distance_ratio"); - param_.min_lookahead_distance = this->declare_parameter("min_lookahead_distance"); - param_.reverse_min_lookahead_distance = - this->declare_parameter("reverse_min_lookahead_distance"); - - // Subscribers - using std::placeholders::_1; - sub_trajectory_ = this->create_subscription( - "input/reference_trajectory", 1, std::bind(&PurePursuitNode::onTrajectory, this, _1)); - sub_current_odometry_ = this->create_subscription( - "input/current_odometry", 1, std::bind(&PurePursuitNode::onCurrentOdometry, this, _1)); - - // Publishers - pub_ctrl_cmd_ = - this->create_publisher("output/control_raw", 1); - - // Debug Publishers - pub_debug_marker_ = - this->create_publisher("~/debug/markers", 0); - - // Timer - { - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(param_.ctrl_period)); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&PurePursuitNode::onTimer, this)); - } - - // Wait for first current pose - tf_utils::waitForTransform(tf_buffer_, "map", "base_link"); -} - -bool PurePursuitNode::isDataReady() -{ - if (!current_odometry_) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_odometry..."); - return false; - } - - if (!trajectory_) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for trajectory..."); - return false; - } - - if (!current_pose_) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_pose..."); - return false; - } - - return true; -} - -void PurePursuitNode::onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - current_odometry_ = msg; -} - -void PurePursuitNode::onTrajectory( - const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) -{ - trajectory_ = msg; -} - -void PurePursuitNode::onTimer() -{ - current_pose_ = self_pose_listener_.getCurrentPose(); - - if (!isDataReady()) { - return; - } - - const auto target_curvature = calcTargetCurvature(); - - if (target_curvature) { - publishCommand(*target_curvature); - publishDebugMarker(); - } else { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "failed to solve pure_pursuit"); - publishCommand({0.0}); - } -} - -void PurePursuitNode::publishCommand(const double target_curvature) -{ - autoware_control_msgs::msg::Lateral cmd; - cmd.stamp = get_clock()->now(); - cmd.steering_tire_angle = - planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); - pub_ctrl_cmd_->publish(cmd); -} - -void PurePursuitNode::publishDebugMarker() const -{ - visualization_msgs::msg::MarkerArray marker_array; - - marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target)); - marker_array.markers.push_back( - createTrajectoryCircleMarker(debug_data_.next_target, current_pose_->pose)); - - pub_debug_marker_->publish(marker_array); -} - -boost::optional PurePursuitNode::calcTargetCurvature() -{ - // Ignore invalid trajectory - if (trajectory_->points.size() < 3) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "received path size is < 3, ignored"); - return {}; - } - - // Calculate target point for velocity/acceleration - const auto target_point = calcTargetPoint(); - if (!target_point) { - return {}; - } - - const double target_vel = target_point->longitudinal_velocity_mps; - - // Calculate lookahead distance - const bool is_reverse = (target_vel < 0); - const double min_lookahead_distance = - is_reverse ? param_.reverse_min_lookahead_distance : param_.min_lookahead_distance; - const double lookahead_distance = calcLookaheadDistance( - current_odometry_->twist.twist.linear.x, param_.lookahead_distance_ratio, - min_lookahead_distance); - - // Set PurePursuit data - pure_pursuit_->setCurrentPose(current_pose_->pose); - pure_pursuit_->setWaypoints(planning_utils::extractPoses(*trajectory_)); - pure_pursuit_->setLookaheadDistance(lookahead_distance); - - // Run PurePursuit - const auto pure_pursuit_result = pure_pursuit_->run(); - if (!pure_pursuit_result.first) { - return {}; - } - - const auto kappa = pure_pursuit_result.second; - - // Set debug data - debug_data_.next_target = pure_pursuit_->getLocationOfNextTarget(); - - return kappa; -} - -boost::optional PurePursuitNode::calcTargetPoint() - const -{ - const auto closest_idx_result = planning_utils::findClosestIdxWithDistAngThr( - planning_utils::extractPoses(*trajectory_), current_pose_->pose, 3.0, M_PI_4); - - if (!closest_idx_result.first) { - RCLCPP_ERROR(get_logger(), "cannot find closest waypoint"); - return {}; - } - - return trajectory_->points.at(closest_idx_result.second); -} -} // namespace pure_pursuit - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(pure_pursuit::PurePursuitNode) diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_viz.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_viz.cpp deleted file mode 100644 index 1778bc6fbe79f..0000000000000 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_viz.cpp +++ /dev/null @@ -1,93 +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. - -/* - * Copyright 2015-2019 Autoware Foundation. 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 "pure_pursuit/pure_pursuit_viz.hpp" - -#include "pure_pursuit/util/marker_helper.hpp" -#include "pure_pursuit/util/planning_utils.hpp" - -#include - -namespace -{ -std::vector generateTrajectoryCircle( - const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose) -{ - constexpr double theta_range = M_PI / 10; - constexpr double step_rad = 0.005; - - const double radius = pure_pursuit::planning_utils::calcRadius(target, current_pose); - - std::vector trajectory_circle; - for (double theta = -theta_range; theta <= theta_range; theta += step_rad) { - geometry_msgs::msg::Point p; - p.x = radius * sin(theta); - p.y = radius * (1 - cos(theta)); - p.z = target.z; - - trajectory_circle.push_back( - pure_pursuit::planning_utils::transformToAbsoluteCoordinate2D(p, current_pose)); - } - - return trajectory_circle; -} - -} // namespace - -namespace pure_pursuit -{ -visualization_msgs::msg::Marker createNextTargetMarker( - const geometry_msgs::msg::Point & next_target) -{ - auto marker = createDefaultMarker( - "map", "next_target", 0, visualization_msgs::msg::Marker::SPHERE, - createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(0.0, 1.0, 0.0, 1.0)); - - marker.pose.position = next_target; - - return marker; -} - -visualization_msgs::msg::Marker createTrajectoryCircleMarker( - const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose) -{ - auto marker = createDefaultMarker( - "map", "trajectory_circle", 0, visualization_msgs::msg::Marker::LINE_STRIP, - createMarkerScale(0.05, 0, 0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); - - const auto trajectory_circle = generateTrajectoryCircle(target, current_pose); - for (auto p : trajectory_circle) { - marker.points.push_back(p); - marker.colors.push_back(marker.color); - } - - return marker; -} -} // namespace pure_pursuit diff --git a/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp b/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp deleted file mode 100644 index 0ee96e970782c..0000000000000 --- a/control/pure_pursuit/src/pure_pursuit_core/pure_pursuit.cpp +++ /dev/null @@ -1,212 +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. -/* - * Copyright 2015-2019 Autoware Foundation. 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 "pure_pursuit/pure_pursuit.hpp" - -#include "pure_pursuit/util/planning_utils.hpp" - -#include -#include -#include -#include - -namespace pure_pursuit -{ -bool PurePursuit::isDataReady() -{ - if (!curr_wps_ptr_) { - return false; - } - if (!curr_pose_ptr_) { - return false; - } - return true; -} - -std::pair PurePursuit::run() -{ - if (!isDataReady()) { - return std::make_pair(false, std::numeric_limits::quiet_NaN()); - } - - auto closest_pair = planning_utils::findClosestIdxWithDistAngThr( - *curr_wps_ptr_, *curr_pose_ptr_, closest_thr_dist_, closest_thr_ang_); - - if (!closest_pair.first) { - RCLCPP_WARN( - logger, "cannot find, curr_bool: %d, closest_idx: %d", closest_pair.first, - closest_pair.second); - return std::make_pair(false, std::numeric_limits::quiet_NaN()); - } - - int32_t next_wp_idx = findNextPointIdx(closest_pair.second); - if (next_wp_idx == -1) { - RCLCPP_WARN(logger, "lost next waypoint"); - return std::make_pair(false, std::numeric_limits::quiet_NaN()); - } - - loc_next_wp_ = curr_wps_ptr_->at(next_wp_idx).position; - - geometry_msgs::msg::Point next_tgt_pos; - // if next waypoint is first - if (next_wp_idx == 0) { - next_tgt_pos = curr_wps_ptr_->at(next_wp_idx).position; - } else { - // linear interpolation - std::pair lerp_pair = lerpNextTarget(next_wp_idx); - - if (!lerp_pair.first) { - RCLCPP_WARN(logger, "lost target! "); - return std::make_pair(false, std::numeric_limits::quiet_NaN()); - } - - next_tgt_pos = lerp_pair.second; - } - loc_next_tgt_ = next_tgt_pos; - - double kappa = planning_utils::calcCurvature(next_tgt_pos, *curr_pose_ptr_); - - return std::make_pair(true, kappa); -} - -// linear interpolation of next target -std::pair PurePursuit::lerpNextTarget(int32_t next_wp_idx) -{ - constexpr double ERROR2 = 1e-5; // 0.00001 - const geometry_msgs::msg::Point & vec_end = curr_wps_ptr_->at(next_wp_idx).position; - const geometry_msgs::msg::Point & vec_start = curr_wps_ptr_->at(next_wp_idx - 1).position; - const geometry_msgs::msg::Pose & curr_pose = *curr_pose_ptr_; - - Eigen::Vector3d vec_a( - (vec_end.x - vec_start.x), (vec_end.y - vec_start.y), (vec_end.z - vec_start.z)); - - if (vec_a.norm() < ERROR2) { - RCLCPP_ERROR(logger, "waypoint interval is almost 0"); - return std::make_pair(false, geometry_msgs::msg::Point()); - } - - const double lateral_error = - planning_utils::calcLateralError2D(vec_start, vec_end, curr_pose.position); - - if (fabs(lateral_error) > lookahead_distance_) { - RCLCPP_ERROR(logger, "lateral error is larger than lookahead distance"); - RCLCPP_ERROR( - logger, "lateral error: %lf, lookahead distance: %lf", lateral_error, lookahead_distance_); - return std::make_pair(false, geometry_msgs::msg::Point()); - } - - /* calculate the position of the foot of a perpendicular line */ - Eigen::Vector2d uva2d(vec_a.x(), vec_a.y()); - uva2d.normalize(); - Eigen::Rotation2Dd rot = - (lateral_error > 0) ? Eigen::Rotation2Dd(-M_PI / 2.0) : Eigen::Rotation2Dd(M_PI / 2.0); - Eigen::Vector2d uva2d_rot = rot * uva2d; - - geometry_msgs::msg::Point h; - h.x = curr_pose.position.x + fabs(lateral_error) * uva2d_rot.x(); - h.y = curr_pose.position.y + fabs(lateral_error) * uva2d_rot.y(); - h.z = curr_pose.position.z; - - // if there is a intersection - if (fabs(fabs(lateral_error) - lookahead_distance_) < ERROR2) { - return std::make_pair(true, h); - } else { - // if there are two intersection - // get intersection in front of vehicle - const double s = sqrt(pow(lookahead_distance_, 2) - pow(lateral_error, 2)); - geometry_msgs::msg::Point res; - res.x = h.x + s * uva2d.x(); - res.y = h.y + s * uva2d.y(); - res.z = curr_pose.position.z; - return std::make_pair(true, res); - } -} - -int32_t PurePursuit::findNextPointIdx(int32_t search_start_idx) -{ - // if waypoints are not given, do nothing. - if (curr_wps_ptr_->empty() || search_start_idx == -1) { - return -1; - } - - // look for the next waypoint. - for (int32_t i = search_start_idx; i < (int32_t)curr_wps_ptr_->size(); i++) { - // if search waypoint is the last - if (i == ((int32_t)curr_wps_ptr_->size() - 1)) { - return i; - } - - // if waypoint direction is forward - const auto gld = planning_utils::getLaneDirection(*curr_wps_ptr_, 0.05); - if (gld == 0) { - // if waypoint is not in front of ego, skip - auto ret = planning_utils::transformToRelativeCoordinate2D( - curr_wps_ptr_->at(i).position, *curr_pose_ptr_); - if (ret.x < 0) { - continue; - } - } else if (gld == 1) { - // waypoint direction is backward - - // if waypoint is in front of ego, skip - auto ret = planning_utils::transformToRelativeCoordinate2D( - curr_wps_ptr_->at(i).position, *curr_pose_ptr_); - if (ret.x > 0) { - continue; - } - } else { - return -1; - } - - const geometry_msgs::msg::Point & curr_motion_point = curr_wps_ptr_->at(i).position; - const geometry_msgs::msg::Point & curr_pose_point = curr_pose_ptr_->position; - // if there exists an effective waypoint - const double ds = planning_utils::calcDistSquared2D(curr_motion_point, curr_pose_point); - if (ds > std::pow(lookahead_distance_, 2)) { - return i; - } - } - - // if this program reaches here , it means we lost the waypoint! - return -1; -} - -void PurePursuit::setCurrentPose(const geometry_msgs::msg::Pose & msg) -{ - curr_pose_ptr_ = std::make_shared(); - *curr_pose_ptr_ = msg; -} - -void PurePursuit::setWaypoints(const std::vector & msg) -{ - curr_wps_ptr_ = std::make_shared>(); - *curr_wps_ptr_ = msg; -} - -} // namespace pure_pursuit diff --git a/control/shift_decider/CMakeLists.txt b/control/shift_decider/CMakeLists.txt deleted file mode 100644 index ca6f4af74f8ca..0000000000000 --- a/control/shift_decider/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(shift_decider) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(shift_decider_node SHARED - src/shift_decider.cpp -) - -rclcpp_components_register_node(shift_decider_node - PLUGIN "ShiftDecider" - EXECUTABLE shift_decider -) - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/shift_decider/README.md b/control/shift_decider/README.md deleted file mode 100644 index c9fdc20696f0e..0000000000000 --- a/control/shift_decider/README.md +++ /dev/null @@ -1,56 +0,0 @@ -# Shift Decider - -## Purpose - -`shift_decider` is a module to decide shift from ackermann control command. - -## Inner-workings / Algorithms - -### Flow chart - -```plantuml -@startuml -skinparam monochrome true - -title update current shift -start -if (absolute target velocity is less than threshold) then (yes) - :set previous shift; -else(no) -if (target velocity is positive) then (yes) - :set shift DRIVE; -else - :set shift REVERSE; -endif -endif - :publish current shift; -note right - publish shift for constant interval -end note -stop -@enduml -``` - -### Algorithms - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| --------------------- | ------------------------------------- | ---------------------------- | -| `~/input/control_cmd` | `autoware_control_msgs::msg::Control` | Control command for vehicle. | - -### Output - -| Name | Type | Description | -| ------------------ | ----------------------------------------- | ---------------------------------- | -| `~output/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | - -## Parameters - -none. - -## Assumptions / Known limits - -TBD. diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp deleted file mode 100644 index b11a0f40625af..0000000000000 --- a/control/shift_decider/include/shift_decider/shift_decider.hpp +++ /dev/null @@ -1,56 +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 SHIFT_DECIDER__SHIFT_DECIDER_HPP_ -#define SHIFT_DECIDER__SHIFT_DECIDER_HPP_ - -#include - -#include -#include -#include -#include - -#include - -class ShiftDecider : public rclcpp::Node -{ -public: - explicit ShiftDecider(const rclcpp::NodeOptions & node_options); - -private: - void onTimer(); - void onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg); - void onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg); - void onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg); - void updateCurrentShiftCmd(); - void initTimer(double period_s); - - rclcpp::Publisher::SharedPtr pub_shift_cmd_; - rclcpp::Subscription::SharedPtr sub_control_cmd_; - rclcpp::Subscription::SharedPtr sub_autoware_state_; - rclcpp::Subscription::SharedPtr sub_current_gear_; - - rclcpp::TimerBase::SharedPtr timer_; - - autoware_control_msgs::msg::Control::SharedPtr control_cmd_; - autoware_system_msgs::msg::AutowareState::SharedPtr autoware_state_; - autoware_vehicle_msgs::msg::GearCommand shift_cmd_; - autoware_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; - uint8_t prev_shift_command = autoware_vehicle_msgs::msg::GearCommand::PARK; - - bool park_on_goal_; -}; - -#endif // SHIFT_DECIDER__SHIFT_DECIDER_HPP_ diff --git a/control/shift_decider/launch/shift_decider.launch.xml b/control/shift_decider/launch/shift_decider.launch.xml deleted file mode 100644 index 73ed434ade7f6..0000000000000 --- a/control/shift_decider/launch/shift_decider.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml deleted file mode 100644 index 885e780c90bcc..0000000000000 --- a/control/shift_decider/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - shift_decider - 0.1.0 - The shift_decider package - Takamasa Horibe - Apache License 2.0 - - Takamasa Horibe - - ament_cmake - autoware_cmake - - autoware_control_msgs - autoware_system_msgs - autoware_vehicle_msgs - rclcpp - rclcpp_components - - ament_cmake_cppcheck - ament_cmake_cpplint - ament_lint_auto - - - ament_cmake - - diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp deleted file mode 100644 index f003513060a34..0000000000000 --- a/control/shift_decider/src/shift_decider.cpp +++ /dev/null @@ -1,109 +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. - -#include "shift_decider/shift_decider.hpp" - -#include - -#include -#include -#include -#include - -ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) -: Node("shift_decider", node_options) -{ - using std::placeholders::_1; - - static constexpr std::size_t queue_size = 1; - rclcpp::QoS durable_qos(queue_size); - durable_qos.transient_local(); - - park_on_goal_ = declare_parameter("park_on_goal"); - - pub_shift_cmd_ = - create_publisher("output/gear_cmd", durable_qos); - sub_control_cmd_ = create_subscription( - "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); - sub_autoware_state_ = create_subscription( - "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1)); - sub_current_gear_ = create_subscription( - "input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1)); - - initTimer(0.1); -} - -void ShiftDecider::onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg) -{ - control_cmd_ = msg; -} - -void ShiftDecider::onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg) -{ - autoware_state_ = msg; -} - -void ShiftDecider::onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg) -{ - current_gear_ptr_ = msg; -} - -void ShiftDecider::onTimer() -{ - if (!autoware_state_ || !control_cmd_ || !current_gear_ptr_) { - return; - } - - updateCurrentShiftCmd(); - pub_shift_cmd_->publish(shift_cmd_); -} - -void ShiftDecider::updateCurrentShiftCmd() -{ - using autoware_system_msgs::msg::AutowareState; - using autoware_vehicle_msgs::msg::GearCommand; - - shift_cmd_.stamp = now(); - static constexpr double vel_threshold = 0.01; // to prevent chattering - if (autoware_state_->state == AutowareState::DRIVING) { - if (control_cmd_->longitudinal.velocity > vel_threshold) { - shift_cmd_.command = GearCommand::DRIVE; - } else if (control_cmd_->longitudinal.velocity < -vel_threshold) { - shift_cmd_.command = GearCommand::REVERSE; - } else { - shift_cmd_.command = prev_shift_command; - } - } else { - if ( - (autoware_state_->state == AutowareState::ARRIVED_GOAL || - autoware_state_->state == AutowareState::WAITING_FOR_ROUTE) && - park_on_goal_) { - shift_cmd_.command = GearCommand::PARK; - } else { - shift_cmd_.command = current_gear_ptr_->report; - } - } - prev_shift_command = shift_cmd_.command; -} - -void ShiftDecider::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(&ShiftDecider::onTimer, this)); -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(ShiftDecider) diff --git a/control/smart_mpc_trajectory_follower/CMakeLists.txt b/control/smart_mpc_trajectory_follower/CMakeLists.txt deleted file mode 100644 index 7aecab2597dcd..0000000000000 --- a/control/smart_mpc_trajectory_follower/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(smart_mpc_trajectory_follower) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -install(PROGRAMS - smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py - DESTINATION lib/${PROJECT_NAME} -) -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/control/smart_mpc_trajectory_follower/README.md b/control/smart_mpc_trajectory_follower/README.md deleted file mode 100644 index 8fcc13142d68b..0000000000000 --- a/control/smart_mpc_trajectory_follower/README.md +++ /dev/null @@ -1,338 +0,0 @@ -

- - - -

- - -# Smart MPC Trajectory Follower - -Smart MPC (Model Predictive Control) is a control algorithm that combines model predictive control and machine learning. While inheriting the advantages of model predictive control, it solves its disadvantage of modeling difficulty with a data-driven method using machine learning. - -This technology makes it relatively easy to operate model predictive control, which is expensive to implement, as long as an environment for collecting data can be prepared. - -

- - - -

- -## Setup - -After building autoware, move to `control/smart_mpc_trajectory_follower` and run the following command: - -```bash -pip3 install . -``` - -If you have already installed and want to update the package, run the following command instead: - -```bash -pip3 install -U . -``` - -## Provided features - -This package provides smart MPC logic for path-following control as well as mechanisms for learning and evaluation. These features are described below. - -### Trajectory following control based on iLQR/MPPI - -The control mode can be selected from "ilqr", "mppi", or "mppi_ilqr", and can be set as `mpc_parameter:system:mode` in [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml). -In "mppi_ilqr" mode, the initial value of iLQR is given by the MPPI solution. - -> [!NOTE] -> With the default settings, the performance of "mppi" mode is limited due to an insufficient number of samples. This issue is being addressed with ongoing work to introduce GPU support. - -To perform a simulation, run the following command: - -```bash -ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-planning vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit trajectory_follower_mode:=smart_mpc_trajectory_follower -``` - -> [!NOTE] -> When running with the nominal model set in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml), set `trained_model_parameter:control_application:use_trained_model` to `false` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). To run using the trained model, set `trained_model_parameter:control_application:use_trained_model` to `true`, but the trained model must have been generated according to the following procedure. - -### Training of model and reflection in control - -To obtain training data, start autoware, perform a drive, and record rosbag data with the following commands. - -```bash -ros2 bag record /localization/kinematic_state /localization/acceleration /vehicle/status/steering_status /control/command/control_cmd /control/trajectory_follower/control_cmd /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw /system/operation_mode/state /vehicle/status/control_mode /sensing/imu/imu_data /debug_mpc_x_des /debug_mpc_y_des /debug_mpc_v_des /debug_mpc_yaw_des /debug_mpc_acc_des /debug_mpc_steer_des /debug_mpc_X_des_converted /debug_mpc_x_current /debug_mpc_error_prediction /debug_mpc_max_trajectory_err /debug_mpc_emergency_stop_mode /debug_mpc_goal_stop_mode /debug_mpc_total_ctrl_time /debug_mpc_calc_u_opt_time -``` - -Move [rosbag2.bash](./smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash) to the rosbag directory recorded above and execute the following command on the directory - -```bash -bash rosbag2.bash -``` - -This converts rosbag data into CSV format for training models. - -> [!NOTE] -> Note that a large number of terminals are automatically opened at runtime, but they are automatically closed after rosbag data conversion is completed. -> From the time you begin this process until all terminals are closed, autoware should not be running. - -Instead, the same result can be obtained by executing the following command in a python environment: - -```python -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model -model_trainer = train_drive_NN_model.train_drive_NN_model() -model_trainer.transform_rosbag_to_csv(rosbag_dir) -``` - -Here, `rosbag_dir` represents the rosbag directory. -At this time, all CSV files in `rosbag_dir` are automatically deleted first. - -The paths of the rosbag directories used for training, `dir_0`, `dir_1`, `dir_2`,... and the directory `save_dir` where you save the models, the model can be saved in the python environment as follows: - -```python -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model -model_trainer = train_drive_NN_model.train_drive_NN_model() -model_trainer.add_data_from_csv(dir_0) -model_trainer.add_data_from_csv(dir_1) -model_trainer.add_data_from_csv(dir_2) -... -model_trainer.get_trained_model() -model_trainer.save_models(save_dir) -``` - -After performing the polynomial regression, the NN can be trained on the residuals as follows: - -```python -model_trainer.get_trained_model(use_polynomial_reg=True) -``` - -> [!NOTE] -> In the default setting, regression is performed by several preselected polynomials. -> When `use_selected_polynomial=False` is set as the argument of get_trained_model, the `deg` argument allows setting the maximum degree of the polynomial to be used. - -If only polynomial regression is performed and no NN model is used, run the following command: - -```python -model_trainer.get_trained_model(use_polynomial_reg=True,force_NN_model_to_zero=True) -``` - -Move `model_for_test_drive.pth` and `polynomial_reg_info.npz` saved in `save_dir` to the home directory and set `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml) to `true` to reflect the trained model in the control. - -### Performance evaluation - -Here, as an example, we describe the verification of the adaptive performance when the wheel base of the sample_vehicle is 2.79 m, but an incorrect value of 2.0 m is given to the controller side. -To give the controller 2.0 m as the wheel base, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.0, and run the following command: - -```bash -python3 -m smart_mpc_trajectory_follower.clear_pycache -``` - -#### Test on autoware - -To perform a control test on autoware with the nominal model before training, make sure that `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml) is `false` and launch autoware in the manner described in "Trajectory following control based on iLQR/MPPI". This time, the following route will be used for the test: - -

- -Record rosbag and train the model in the manner described in "Training of model and reflection in control", and move the generated files `model_for_test_drive.pth` and `polynomial_reg_info.npz` to the home directory. - -> [!NOTE] -> Although the data used for training is small, for the sake of simplicity, we will see how much performance can be improved with this amount of data. - -To control using the trained model obtained here, set `trained_model_parameter:control_application:use_trained_model` to `true`, start autoware in the same way, and drive the same route recording rosbag. -After the driving is complete, convert the rosbag file to CSV format using the method described in "Training of model and reflection in control". -A plot of the lateral deviation is obtained by running the `lateral_error_visualize` function in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb` for the nominal and training model rosbag files `rosbag_nominal` and `rosbag_trained`, respectively, as follows: - -```python -lateral_error_visualize(dir_name=rosbag_nominal,ylim=[-1.2,1.2]) -lateral_error_visualize(dir_name=rosbag_trained,ylim=[-1.2,1.2]) -``` - -The following results were obtained. - -
- - -
- -#### Test on python simulator - -First, to give wheel base 2.79 m in the python simulator, create the following file and save it in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator` with the name `sim_setting.json`: - -```json -{ "wheel_base": 2.79 } -``` - -Next, run the following commands to test the slalom driving on the python simulator with the nominal model: - -```python -import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model -initial_error = [0.0, 0.03, 0.01, -0.01, 0.0, 0.0] -save_dir = "test_python_sim" -python_simulator.slalom_drive(save_dir=save_dir,use_trained_model=False,initial_error=initial_error) -``` - -Here, `initial_error` is the initial error from the target trajectory, in the order of x-coordinate, y-coordinate, longitudinal velocity, yaw angle, longitudinal acceleration, and steer angle, -and `save_dir` is the directory where the driving test results are saved. - -> [!NOTE] -> The value of `use_trained_model` given as the argument of `python_simulator.slalom_drive` takes precedence over the value of `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). - -Run the following commands to perform training using driving data of the nominal model. - -```python -model_trainer = train_drive_NN_model.train_drive_NN_model() -model_trainer.add_data_from_csv(save_dir) -model_trainer.save_train_data(save_dir) -model_trainer.get_trained_model(use_polynomial_reg=True) -model_trainer.save_models(save_dir=save_dir) -``` - -This way, files `model_for_test_drive.pth` and `polynomial_reg_info.npz` are saved in `save_dir`. -The following results were obtained. - -

- -

- -The center of the upper row represents the lateral deviation. - -Finally, to drive with the training model, run the following commands: - -```python -load_dir = save_dir -save_dir = "test_python_trained_sim" -python_simulator.slalom_drive(save_dir=save_dir,load_dir=load_dir,use_trained_model=True,initial_error=initial_error) -``` - -The following results were obtained. - -

- -

- -It can be seen that the lateral deviation has improved significantly. - -Here we have described wheel base, but the parameters that can be passed to the python simulator are as follows. - -| Parameter | Type | Description | -| ------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| steer_bias | float | steer bias [rad] | -| steer_rate_lim | float | steer rate limit [rad/s] | -| vel_rate_lim | float | acceleration limit [m/s^2] | -| wheel_base | float | wheel base [m] | -| steer_dead_band | float | steer dead band [rad] | -| adaptive_gear_ratio_coef | list[float] | List of floats of length 6 specifying information on speed-dependent gear ratios from tire angle to steering wheel angle. | -| acc_time_delay | float | acceleration time delay [s] | -| steer_time_delay | float | steer time delay [s] | -| acc_time_constant | float | acceleration time constant [s] | -| steer_time_constant | float | steer time constant [s] | -| accel_map_scale | float | Parameter that magnifies the corresponding distortion from acceleration input values to actual acceleration realizations.
Correspondence information is kept in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv`. | -| acc_scaling | float | acceleration scaling | -| steer_scaling | float | steer scaling | - -For example, to give the simulation side 0.01 [rad] of steer bias and 0.001 [rad] of steer dead band, edit the `sim_setting.json` as follows. - -```json -{ "steer_bias": 0.01, "steer_dead_band": 0.001 } -``` - -#### Auto test on python simulator - -Here, we describe a method for testing adaptive performance by giving the simulation side a predefined range of model parameters while the control side is given constant model parameters. - -First, to restore nominal model settings to default values, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.79, and run the following command: - -```bash -python3 -m smart_mpc_trajectory_follower.clear_pycache -``` - -To run a driving experiment within the parameter change range set in [run_sim.py](./smart_mpc_trajectory_follower/python_simulator/run_sim.py), for example, move to `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator` and run the following command: - -```bash -python3 run_sim.py --param_name steer_bias -``` - -Here we described the experimental procedure for steer bias, and the same method can be used for other parameters. - -If you want to do it for all parameters at once, run the following command: - -```bash -yes | python3 run_sim.py -``` - -In `run_sim.py`, the following parameters can be set: - -| Parameter | Type | Description | -| ------------------------- | ---- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| USE_TRAINED_MODEL_DIFF | bool | Whether the derivative of the trained model is reflected in the control | -| DATA_COLLECTION_MODE | str | Which method will be used to collect the training data　
"ff": Straight line driving with feed-forward input
"pp": Figure eight driving with pure pursuit control
"mpc": Slalom driving with mpc | -| USE_POLYNOMIAL_REGRESSION | bool | Whether to perform polynomial regression before NN | -| USE_SELECTED_POLYNOMIAL | bool | When USE_POLYNOMIAL_REGRESSION is True, perform polynomial regression using only some preselected polynomials.
The choice of polynomials is intended to be able to absorb the contribution of some parameter shifts based on the nominal model of the vehicle. | -| FORCE_NN_MODEL_TO_ZERO | bool | Whether to force the NN model to zero (i.e., erase the contribution of the NN model).
When USE_POLYNOMIAL_REGRESSION is True, setting FORCE_MODEL_TO_ZERO to True allows the control to reflect the results of polynomial regression only, without using NN models. | -| FIT_INTERCEPT | bool | Whether to include bias in polynomial regression.
If it is False, perform the regression with a polynomial of the first degree or higher. | -| USE_INTERCEPT | bool | When a polynomial regression including bias is performed, whether to use or discard the resulting bias information.
It is meaningful only if FIT_INTERCEPT is True.
If it is False, discard the bias in the polynomial regression in the hope that the NN model can remove the bias term, even if the polynomial regression is performed with the bias included. | - -> [!NOTE] -> When `run_sim.py` is run, the `use_trained_model_diff` set in `run_sim.py` takes precedence over the `trained_model_parameter:control_application:use_trained_model_diff` set in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). - -## Change of nominal parameters and their reloading - -The nominal parameters of vehicle model can be changed by editing the file [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml). -After changing the nominal parameters, the cache must be deleted by running the following command: - -```bash -python3 -m smart_mpc_trajectory_follower.clear_pycache -``` - -The nominal parameters include the following: - -| Parameter | Type | Description | -| ------------------------------------------------ | ----- | ------------------------------ | -| nominal_parameter:vehicle_info:wheel_base | float | wheel base [m] | -| nominal_parameter:acceleration:acc_time_delay | float | acceleration time delay [s] | -| nominal_parameter:acceleration:acc_time_constant | float | acceleration time constant [s] | -| nominal_parameter:steering:steer_time_delay | float | steer time delay [s] | -| nominal_parameter:steering:steer_time_constant | float | steer time constant [s] | - -## Change of control parameters and their reloading - -The control parameters can be changed by editing files [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml) and [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). -Although it is possible to reflect parameter changes by restarting autoware, the following command allows us to do so without leaving autoware running: - -```bash -ros2 topic pub /pympc_reload_mpc_param_trigger std_msgs/msg/String "data: ''" --once -``` - -The main parameters among the control parameters are as follows. - -### `mpc_param.yaml` - -| Parameter | Type | Description | -| ------------------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| mpc_parameter:system:mode | str | control mode
"ilqr": iLQR mode
"mppi": MPPI mode
"mppi_ilqr": the initial value of iLQR is given by the MPPI solution. | -| mpc_parameter:cost_parameters:Q | list[float] | Stage cost for states.
List of length 8, in order: straight deviation, lateral deviation, velocity deviation, yaw angle deviation, acceleration deviation, steer deviation, acceleration input deviation, steer input deviation cost weights. | -| mpc_parameter:cost_parameters:Q_c | list[float] | Cost in the horizon corresponding to the following timing_Q_c for the states.
The correspondence of the components of the list is the same as for Q. | -| mpc_parameter:cost_parameters:Q_f | list[float] | Termination cost for the states.
The correspondence of the components of the list is the same as for Q. | -| mpc_parameter:cost_parameters:R | list[float] | A list of length 2 where R[0] is weight of cost for the change rate of acceleration input value and R[1] is weight of cost for the change rate of steer input value. | -| mpc_parameter:mpc_setting:timing_Q_c | list[int] | Horizon numbers such that the stage cost for the states is set to Q_c. | - -### `trained_model_param.yaml` - -| Parameter | Type | Description | -| ------------------------------------------------------------------ | ---- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| trained_model_parameter:control_application:use_trained_model | bool | Whether the trained model is reflected in the control or not. | -| trained_model_parameter:control_application:use_trained_model_diff | bool | Whether the derivative of the trained model is reflected on the control or not.
It is meaningful only when use_trained_model is True, and if False, the nominal model is used for the derivative of the dynamics, and trained model is used only for prediction. | - -## Request to release the slow stop mode - -If the predicted trajectory deviates too far from the target trajectory, the system enters a slow stop mode and the vehicle stops moving. -To cancel the slow stop mode and make the vehicle ready to run again, run the following command: - -```bash -ros2 topic pub /pympc_stop_mode_reset_request std_msgs/msg/String "data: ''" --once -``` - -## Limitation - -- May not be able to start when initial position/posture is far from the target. - -- It may take some time until the end of the planning to compile numba functions at the start of the first control. - -- In the stopping action near the goal our control switches to another simple control law. As a result, the stopping action may not work except near the goal. Stopping is also difficult if the acceleration map is significantly shifted. diff --git a/control/smart_mpc_trajectory_follower/package.xml b/control/smart_mpc_trajectory_follower/package.xml deleted file mode 100644 index 6cdd689c8bd5a..0000000000000 --- a/control/smart_mpc_trajectory_follower/package.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - smart_mpc_trajectory_follower - 1.0.0 - Nodes to follow a trajectory by generating control commands using smart mpc - - - Masayuki Aino - - Apache License 2.0 - - Masayuki Aino - - ament_cmake_auto - ament_cmake_python - autoware_cmake - - autoware_adapi_v1_msgs - autoware_control_msgs - autoware_planning_msgs - autoware_system_msgs - autoware_vehicle_msgs - motion_utils - pybind11_vendor - python3-scipy - rclcpp - rclcpp_components - tier4_autoware_utils - - ros2launch - - ament_cmake_ros - ament_index_python - ament_lint_auto - autoware_lint_common - autoware_testing - ros_testing - - - ament_cmake - - diff --git a/control/smart_mpc_trajectory_follower/setup.py b/control/smart_mpc_trajectory_follower/setup.py deleted file mode 100644 index 4dca4d72929a8..0000000000000 --- a/control/smart_mpc_trajectory_follower/setup.py +++ /dev/null @@ -1,37 +0,0 @@ -# cspell: ignore numba -import glob -import json -import os -from pathlib import Path - -from setuptools import find_packages -from setuptools import setup - -os.system("pip3 install numba==0.58.1 --force-reinstall") -os.system("pip3 install pybind11") -os.system("pip3 install GPy") -os.system("pip3 install torch") -package_path = {} -package_path["path"] = str(Path(__file__).parent) -with open("smart_mpc_trajectory_follower/package_path.json", "w") as f: - json.dump(package_path, f) -build_cpp_command = "g++ -Ofast -Wall -shared -std=c++11 -fPIC $(python3 -m pybind11 --includes) " -build_cpp_command += "smart_mpc_trajectory_follower/scripts/proxima_calc.cpp " -build_cpp_command += ( - "-o smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) " -) -build_cpp_command += "-lrt -I/usr/include/eigen3" -os.system(build_cpp_command) - -so_path = ( - "scripts/" - + glob.glob("smart_mpc_trajectory_follower/scripts/proxima_calc.*.so")[0].split("/")[-1] -) -setup( - name="smart_mpc_trajectory_follower", - version="1.0.0", - packages=find_packages(), - package_data={ - "smart_mpc_trajectory_follower": ["package_path.json", so_path], - }, -) diff --git a/control/trajectory_follower_base/CMakeLists.txt b/control/trajectory_follower_base/CMakeLists.txt deleted file mode 100644 index 98f86e468e819..0000000000000 --- a/control/trajectory_follower_base/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(trajectory_follower_base) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/longitudinal_controller_base.cpp - src/lateral_controller_base.cpp -) - -ament_auto_package() diff --git a/control/trajectory_follower_base/README.md b/control/trajectory_follower_base/README.md deleted file mode 100644 index 6bd3d74e75271..0000000000000 --- a/control/trajectory_follower_base/README.md +++ /dev/null @@ -1,53 +0,0 @@ -# Trajectory Follower - -This is the design document for the `trajectory_follower` package. - -## Purpose / Use cases - - - - -This package provides the interface of longitudinal and lateral controllers used by the node of the `trajectory_follower_node` package. -We can implement a detailed controller by deriving the longitudinal and lateral base interfaces. - -## Design - -There are lateral and longitudinal base interface classes and each algorithm inherits from this class to implement. -The interface class has the following base functions. - -- `isReady()`: Check if the control is ready to compute. -- `run()`: Compute control commands and return to [Trajectory Follower Nodes](../trajectory_follower_node/README.md). This must be implemented by inherited algorithms. -- `sync()`: Input the result of running the other controller. - - steer angle convergence - - allow keeping stopped until steer is converged. - - velocity convergence(currently not used) - -See [the Design of Trajectory Follower Nodes](../trajectory_follower_node/README.md#Design) for how these functions work in the node. - -## Separated lateral (steering) and longitudinal (velocity) controls - -This longitudinal controller assumes that the roles of lateral and longitudinal control are separated as follows. - -- Lateral control computes a target steering to keep the vehicle on the trajectory, assuming perfect velocity tracking. -- Longitudinal control computes a target velocity/acceleration to keep the vehicle velocity on the trajectory speed, assuming perfect trajectory tracking. - -Ideally, dealing with the lateral and longitudinal control as a single mixed problem can achieve high performance. In contrast, there are two reasons to provide velocity controller as a stand-alone function, described below. - -### Complex requirements for longitudinal motion - -The longitudinal vehicle behavior that humans expect is difficult to express in a single logic. For example, the expected behavior just before stopping differs depending on whether the ego-position is ahead/behind of the stop line, or whether the current speed is higher/lower than the target speed to achieve a human-like movement. - -In addition, some vehicles have difficulty measuring the ego-speed at extremely low speeds. In such cases, a configuration that can improve the functionality of the longitudinal control without affecting the lateral control is important. - -There are many characteristics and needs that are unique to longitudinal control. Designing them separately from the lateral control keeps the modules less coupled and improves maintainability. - -### Nonlinear coupling of lateral and longitudinal motion - -The lat-lon mixed control problem is very complex and uses nonlinear optimization to achieve high performance. Since it is difficult to guarantee the convergence of the nonlinear optimization, a simple control logic is also necessary for development. - -Also, the benefits of simultaneous longitudinal and lateral control are small if the vehicle doesn't move at high speed. - -## Related issues - - diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp deleted file mode 100644 index c2419427ac961..0000000000000 --- a/control/trajectory_follower_base/include/trajectory_follower_base/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021 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 TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ -#define TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport) -#define TRAJECTORY_FOLLOWER_LOCAL -#else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport) -#define TRAJECTORY_FOLLOWER_LOCAL -#endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#elif defined(__linux__) -#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) -#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) -#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // TRAJECTORY_FOLLOWER_BASE__VISIBILITY_CONTROL_HPP_ diff --git a/control/trajectory_follower_base/package.xml b/control/trajectory_follower_base/package.xml deleted file mode 100644 index 6f2e9c3e8ebc2..0000000000000 --- a/control/trajectory_follower_base/package.xml +++ /dev/null @@ -1,50 +0,0 @@ - - - - trajectory_follower_base - 1.0.0 - Library for generating lateral and longitudinal controls following a trajectory - - - Takamasa Horibe - - Takayuki Murooka - - Apache 2.0 - - Takamasa Horibe - Maxime CLEMENT - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - autoware_adapi_v1_msgs - autoware_control_msgs - autoware_planning_msgs - autoware_vehicle_msgs - diagnostic_msgs - diagnostic_updater - eigen - geometry_msgs - interpolation - motion_utils - osqp_interface - rclcpp - rclcpp_components - std_msgs - tf2 - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - vehicle_info_util - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - eigen - - - ament_cmake - - diff --git a/control/trajectory_follower_node/CMakeLists.txt b/control/trajectory_follower_node/CMakeLists.txt deleted file mode 100644 index 653b02eb39ed6..0000000000000 --- a/control/trajectory_follower_node/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(trajectory_follower_node) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -set(CONTROLLER_NODE controller_node) -ament_auto_add_library(${CONTROLLER_NODE} SHARED - include/trajectory_follower_node/controller_node.hpp - src/controller_node.cpp -) - -rclcpp_components_register_node(${CONTROLLER_NODE} - PLUGIN "autoware::motion::control::trajectory_follower_node::Controller" - EXECUTABLE ${CONTROLLER_NODE}_exe -) - -# simple trajectory follower -set(SIMPLE_TRAJECTORY_FOLLOWER_NODE simple_trajectory_follower) -ament_auto_add_library(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} SHARED - include/trajectory_follower_node/simple_trajectory_follower.hpp - src/simple_trajectory_follower.cpp -) - -rclcpp_components_register_node(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} - PLUGIN "simple_trajectory_follower::SimpleTrajectoryFollower" - EXECUTABLE ${SIMPLE_TRAJECTORY_FOLLOWER_NODE}_exe -) - -if(BUILD_TESTING) - set(TRAJECTORY_FOLLOWER_NODES_TEST test_trajectory_follower_node) - ament_add_ros_isolated_gtest(${TRAJECTORY_FOLLOWER_NODES_TEST} - test/trajectory_follower_test_utils.hpp - test/test_controller_node.cpp - ) - ament_target_dependencies(${TRAJECTORY_FOLLOWER_NODES_TEST} fake_test_node) - target_link_libraries( - ${TRAJECTORY_FOLLOWER_NODES_TEST} ${CONTROLLER_NODE}) - - find_package(autoware_testing REQUIRED) - # smoke test for MPC controller - add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe - PARAM_FILENAMES "lateral/mpc.param.yaml longitudinal/pid.param.yaml -trajectory_follower_node.param.yaml" - TEST_PARAM_FILENAMES "test_controller_mpc.param.yaml test_controller_pid.param.yaml -test_vehicle_info.param.yaml test_nearest_search.param.yaml" - TARGET_INFIX "mpc" - ) - # smoke test for pure pursuit controller - add_smoke_test(${PROJECT_NAME} ${CONTROLLER_NODE}_exe - PARAM_FILENAMES "lateral/pure_pursuit.param.yaml longitudinal/pid.param.yaml -trajectory_follower_node.param.yaml" - TEST_PARAM_FILENAMES "test_controller_pure_pursuit.param.yaml test_controller_pid.param.yaml -test_vehicle_info.param.yaml test_nearest_search.param.yaml" - TARGET_INFIX "pure_pursuit" - ) - -endif() - -ament_auto_package( - INSTALL_TO_SHARE - param - test - launch - config -) diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md deleted file mode 100644 index 3496284efd670..0000000000000 --- a/control/trajectory_follower_node/README.md +++ /dev/null @@ -1,156 +0,0 @@ -# Trajectory Follower Nodes - -## Purpose - -Generate control commands to follow a given Trajectory. - -## Design - -This is a node of the functionalities implemented in the controller class derived from [trajectory_follower_base](../trajectory_follower_base/README.md#trajectory-follower) package. It has instances of those functionalities, gives them input data to perform calculations, and publishes control commands. - -By default, the controller instance with the `Controller` class as follows is used. - -```plantuml -@startuml -package trajectory_follower_base { -abstract class LateralControllerBase { -longitudinal_sync_data_ - - virtual isReady(InputData) - virtual run(InputData) - sync(LongitudinalSyncData) - reset() - -} -abstract class LongitudinalControllerBase { -lateral_sync_data_ - - virtual isReady(InputData) - virtual run(InputData) - sync(LateralSyncData) - reset() - -} - -struct InputData { -trajectory -odometry -steering -accel -} -struct LongitudinalSyncData { -is_steer_converged -} -struct LateralSyncData { -} -} - -package mpc_lateral_controller { -class MPCLateralController { -isReady(InputData) override -run(InputData) override -} -} -package pure_pursuit { -class PurePursuitLateralController { -isReady(InputData) override -run(InputData) override -} -} -package pid_longitudinal_controller { -class PIDLongitudinalController { -isReady(InputData) override -run(InputData) override -} -} - -package trajectory_follower_node { -class Controller { -longitudinal_controller_ -lateral_controller_ -onTimer() -createInputData(): InputData -} -} - -MPCLateralController --|> LateralControllerBase -PurePursuitLateralController --|> LateralControllerBase -PIDLongitudinalController --|> LongitudinalControllerBase - -LateralSyncData --> LongitudinalControllerBase -LateralSyncData --> LateralControllerBase -LongitudinalSyncData --> LongitudinalControllerBase -LongitudinalSyncData --> LateralControllerBase -InputData ..> LateralControllerBase -InputData ..> LongitudinalControllerBase - -LateralControllerBase --o Controller -LongitudinalControllerBase --o Controller -InputData ..> Controller -@enduml -``` - -The process flow of `Controller` class is as follows. - -```cpp -// 1. create input data -const auto input_data = createInputData(*get_clock()); -if (!input_data) { - return; -} - -// 2. check if controllers are ready -const bool is_lat_ready = lateral_controller_->isReady(*input_data); -const bool is_lon_ready = longitudinal_controller_->isReady(*input_data); -if (!is_lat_ready || !is_lon_ready) { - return; -} - -// 3. run controllers -const auto lat_out = lateral_controller_->run(*input_data); -const auto lon_out = longitudinal_controller_->run(*input_data); - -// 4. sync with each other controllers -longitudinal_controller_->sync(lat_out.sync_data); -lateral_controller_->sync(lon_out.sync_data); - -// 5. publish control command -control_cmd_pub_->publish(out); -``` - -Giving the longitudinal controller information about steer convergence allows it to control steer when stopped if following parameters are `true` - -- lateral controller - - `keep_steer_control_until_converged` -- longitudinal controller - - `enable_keep_stopped_until_steer_convergence` - -### Inputs / Outputs / API - -#### Inputs - -- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. -- `nav_msgs/Odometry`: current odometry -- `autoware_vehicle_msgs/SteeringReport` current steering - -#### Outputs - -- `autoware_control_msgs/Control`: message containing both lateral and longitudinal commands. - -#### Parameter - -- `ctrl_period`: control commands publishing period -- `timeout_thr_sec`: duration in second after which input messages are discarded. - - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `Control` if the following two conditions are met. - 1. Both commands have been received. - 2. The last received commands are not older than defined by `timeout_thr_sec`. -- `lateral_controller_mode`: `mpc` or `pure_pursuit` - - (currently there is only `PID` for longitudinal controller) - -## Debugging - -Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. - -A configuration file for [PlotJuggler](https://github.com/facontidavide/PlotJuggler) is provided in the `config` folder which, when loaded, allow to automatically subscribe and visualize information useful for debugging. - -In addition, the predicted MPC trajectory is published on topic `output/lateral/predicted_trajectory` and can be visualized in Rviz. 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 deleted file mode 100644 index 8e9752ba19f66..0000000000000 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ /dev/null @@ -1,132 +0,0 @@ -// Copyright 2021 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 TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ -#define TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#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 "tier4_autoware_utils/system/stop_watch.hpp" -#include "trajectory_follower_base/lateral_controller_base.hpp" -#include "trajectory_follower_base/longitudinal_controller_base.hpp" -#include "trajectory_follower_node/visibility_control.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include -#include -#include - -#include "autoware_control_msgs/msg/control.hpp" -#include "autoware_control_msgs/msg/longitudinal.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/accel_stamped.hpp" -#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/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 -#include -#include -#include - -namespace autoware::motion::control -{ -using trajectory_follower::LateralOutput; -using trajectory_follower::LongitudinalOutput; -namespace trajectory_follower_node -{ - -using autoware_adapi_v1_msgs::msg::OperationModeState; -using tier4_autoware_utils::StopWatch; -using tier4_debug_msgs::msg::Float64Stamped; - -namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; - -/// \classController -/// \brief The node class used for generating longitudinal control commands (velocity/acceleration) -class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node -{ -public: - explicit Controller(const rclcpp::NodeOptions & node_options); - virtual ~Controller() {} - -private: - rclcpp::TimerBase::SharedPtr timer_control_; - double timeout_thr_sec_; - boost::optional longitudinal_output_{boost::none}; - - std::shared_ptr longitudinal_controller_; - std::shared_ptr lateral_controller_; - - rclcpp::Subscription::SharedPtr sub_ref_path_; - rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr sub_steering_; - rclcpp::Subscription::SharedPtr sub_accel_; - rclcpp::Subscription::SharedPtr sub_operation_mode_; - rclcpp::Publisher::SharedPtr control_cmd_pub_; - rclcpp::Publisher::SharedPtr pub_processing_time_lat_ms_; - rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; - rclcpp::Publisher::SharedPtr debug_marker_pub_; - - autoware_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; - nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_; - autoware_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; - geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr_; - OperationModeState::SharedPtr current_operation_mode_ptr_; - - enum class LateralControllerMode { - INVALID = 0, - MPC = 1, - PURE_PURSUIT = 2, - }; - enum class LongitudinalControllerMode { - INVALID = 0, - PID = 1, - }; - - /** - * @brief compute control command, and publish periodically - */ - boost::optional createInputData(rclcpp::Clock & clock) const; - void callbackTimerControl(); - void onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr); - void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); - void onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg); - void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); - bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out); - LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; - LongitudinalControllerMode getLongitudinalControllerMode( - const std::string & algorithm_name) const; - void publishDebugMarker( - const trajectory_follower::InputData & input_data, - const trajectory_follower::LateralOutput & lat_out) const; - - std::unique_ptr logger_configure_; - - std::unique_ptr published_time_publisher_; - - void publishProcessingTime( - const double t_ms, const rclcpp::Publisher::SharedPtr pub); - StopWatch stop_watch_; -}; -} // namespace trajectory_follower_node -} // namespace autoware::motion::control - -#endif // TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp deleted file mode 100644 index e744243969cab..0000000000000 --- a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2022 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 TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ -#define TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ - -#include - -#include -#include -#include -#include -#include -#include - -#include - -namespace simple_trajectory_follower -{ -using autoware_control_msgs::msg::Control; -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; -using geometry_msgs::msg::Pose; -using geometry_msgs::msg::Twist; -using nav_msgs::msg::Odometry; - -class SimpleTrajectoryFollower : public rclcpp::Node -{ -public: - explicit SimpleTrajectoryFollower(const rclcpp::NodeOptions & options); - ~SimpleTrajectoryFollower() = default; - -private: - rclcpp::Subscription::SharedPtr sub_kinematics_; - rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Publisher::SharedPtr pub_cmd_; - rclcpp::TimerBase::SharedPtr timer_; - - Trajectory::SharedPtr trajectory_; - Odometry::SharedPtr odometry_; - TrajectoryPoint closest_traj_point_; - bool use_external_target_vel_; - double external_target_vel_; - double lateral_deviation_; - - void onTimer(); - bool checkData(); - void updateClosest(); - double calcSteerCmd(); - double calcAccCmd(); -}; - -} // namespace simple_trajectory_follower - -#endif // TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp deleted file mode 100644 index 36446c4144a0a..0000000000000 --- a/control/trajectory_follower_node/include/trajectory_follower_node/visibility_control.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2021 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 TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ -#define TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllexport) -#define TRAJECTORY_FOLLOWER_LOCAL -#else // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#define TRAJECTORY_FOLLOWER_PUBLIC __declspec(dllimport) -#define TRAJECTORY_FOLLOWER_LOCAL -#endif // defined(TRAJECTORY_FOLLOWER_BUILDING_DLL) || defined(TRAJECTORY_FOLLOWER_EXPORTS) -#elif defined(__linux__) -#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) -#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define TRAJECTORY_FOLLOWER_PUBLIC __attribute__((visibility("default"))) -#define TRAJECTORY_FOLLOWER_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // TRAJECTORY_FOLLOWER_NODE__VISIBILITY_CONTROL_HPP_ diff --git a/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml b/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml deleted file mode 100644 index 0c4c3faac73a9..0000000000000 --- a/control/trajectory_follower_node/launch/simple_trajectory_follower.launch.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml deleted file mode 100644 index 65446dfb3dd01..0000000000000 --- a/control/trajectory_follower_node/package.xml +++ /dev/null @@ -1,50 +0,0 @@ - - - - trajectory_follower_node - 1.0.0 - Nodes to follow a trajectory by generating control commands separated into lateral and longitudinal commands - - - Takamasa Horibe - - Takayuki Murooka - - Apache License 2.0 - - Takamasa Horibe - Maxime CLEMENT - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - autoware_adapi_v1_msgs - autoware_control_msgs - autoware_planning_msgs - autoware_vehicle_msgs - motion_utils - mpc_lateral_controller - pid_longitudinal_controller - pure_pursuit - rclcpp - rclcpp_components - tier4_autoware_utils - trajectory_follower_base - vehicle_info_util - visualization_msgs - - ros2launch - - ament_cmake_ros - ament_index_python - ament_lint_auto - autoware_lint_common - autoware_testing - fake_test_node - ros_testing - - - ament_cmake - - diff --git a/control/vehicle_cmd_gate/CMakeLists.txt b/control/vehicle_cmd_gate/CMakeLists.txt deleted file mode 100644 index c5db663ccc77e..0000000000000 --- a/control/vehicle_cmd_gate/CMakeLists.txt +++ /dev/null @@ -1,55 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(vehicle_cmd_gate) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(vehicle_cmd_gate_node SHARED - src/vehicle_cmd_gate.cpp - src/vehicle_cmd_filter.cpp - src/adapi_pause_interface.cpp - src/moderate_stop_interface.cpp -) - -rclcpp_components_register_node(vehicle_cmd_gate_node - PLUGIN "vehicle_cmd_gate::VehicleCmdGate" - EXECUTABLE vehicle_cmd_gate_exe -) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "msg/IsFilterActivated.msg" - DEPENDENCIES builtin_interfaces -) - -# to use same package defined message -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(vehicle_cmd_gate_node - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(vehicle_cmd_gate_node "${cpp_typesupport_target}") -endif() - - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_vehicle_cmd_gate - test/src/test_main.cpp - test/src/test_vehicle_cmd_filter.cpp - test/src/test_filter_in_vehicle_cmd_gate_node.cpp - ) - ament_target_dependencies(test_vehicle_cmd_gate - rclcpp - tier4_control_msgs - ) - target_link_libraries(test_vehicle_cmd_gate - vehicle_cmd_gate_node - ) -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml deleted file mode 100644 index e01d566c37df9..0000000000000 --- a/control/vehicle_cmd_gate/package.xml +++ /dev/null @@ -1,50 +0,0 @@ - - - - vehicle_cmd_gate - 0.1.0 - The vehicle_cmd_gate package - Takamasa Horibe - Tomoya Kimura - Apache License 2.0 - - Hiroki OTA - - ament_cmake - autoware_cmake - - rosidl_default_generators - - autoware_adapi_v1_msgs - autoware_control_msgs - autoware_vehicle_msgs - component_interface_specs - component_interface_utils - diagnostic_updater - geometry_msgs - motion_utils - rclcpp - rclcpp_components - std_srvs - tier4_api_utils - tier4_autoware_utils - tier4_control_msgs - tier4_debug_msgs - tier4_external_api_msgs - tier4_system_msgs - tier4_vehicle_msgs - vehicle_info_util - visualization_msgs - - rosidl_default_runtime - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/control/vehicle_cmd_gate/src/marker_helper.hpp b/control/vehicle_cmd_gate/src/marker_helper.hpp deleted file mode 100644 index 44c43a3630151..0000000000000 --- a/control/vehicle_cmd_gate/src/marker_helper.hpp +++ /dev/null @@ -1,119 +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 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/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index 926fbb7435f3a..eaa07f2317940 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace perception_diagnostics @@ -36,7 +37,11 @@ enum class Metric { SIZE, }; +// Each metric has a different return type. (statistic or just a one value etc). +// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant using MetricStatMap = std::unordered_map>; +using MetricValueMap = std::unordered_map; +using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index ec435b0a5e17f..a9b1388281ce8 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -96,7 +96,7 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return map of string describing the requested metric and the calculated value */ - std::optional calculate(const Metric & metric) const; + std::optional calculate(const Metric & metric) const; /** * @brief set the dynamic objects used to calculate obstacle metrics @@ -143,7 +143,7 @@ class MetricsCalculator PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; - MetricStatMap calcObjectsCountMetrics() const; + MetricValueMap calcObjectsCountMetrics() const; bool hasPassedTime(const rclcpp::Time stamp) const; bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index daaea56178873..1bc427e667a2a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -61,6 +61,8 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node DiagnosticStatus generateDiagnosticStatus( const std::string metric, const Stat & metric_stat) const; + DiagnosticStatus generateDiagnosticStatus( + const std::string & metric, const double metric_value) const; private: // Subscribers and publishers diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp index be335a2d78fa3..b05bb5e1d8ad3 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -15,7 +15,7 @@ #ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ #define PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ -#include +#include #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include @@ -45,10 +45,10 @@ inline int64_t bitShift(int64_t original_id) void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); MarkerArray createPointsMarkerArray( diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index 3f855e2f1f887..d64d3a5ec4957 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_perception_msgs + autoware_vehicle_info_utils diagnostic_msgs eigen geometry_msgs @@ -32,7 +33,6 @@ tf2 tf2_ros tier4_autoware_utils - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 9c1e0667e4fef..cc455445ca9f8 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -26,7 +26,7 @@ namespace perception_diagnostics using object_recognition_utils::convertLabelToString; using tier4_autoware_utils::inverseTransformPoint; -std::optional MetricsCalculator::calculate(const Metric & metric) const +std::optional MetricsCalculator::calculate(const Metric & metric) const { // clang-format off const bool use_past_objects = metric == Metric::lateral_deviation || @@ -455,15 +455,14 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas return metric_stat_map; } -MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const +MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const { - MetricStatMap metric_stat_map; + MetricValueMap metric_stat_map; // calculate the average number of objects in the detection area in all past frames const auto overall_average_count = detection_counter_.getOverallAverageCount(); for (const auto & [label, range_and_count] : overall_average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } // calculate the average number of objects in the detection area in the past @@ -472,8 +471,8 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const detection_counter_.getAverageCount(parameters_->objects_count_window_seconds); for (const auto & [label, range_and_count] : average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] - .add(count); + metric_stat_map + ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } @@ -481,8 +480,7 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const const auto total_count = detection_counter_.getTotalCount(); for (const auto & [label, range_and_count] : total_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 0fcdd77f4d515..2933afdab3d08 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -69,16 +69,25 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() // calculate metrics for (const Metric & metric : parameters_->metrics) { - const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); - if (!metric_stat_map.has_value()) { + const auto metric_result = metrics_calculator_.calculate(Metric(metric)); + if (!metric_result.has_value()) { continue; } - for (const auto & [metric, stat] : metric_stat_map.value()) { - if (stat.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); - } - } + std::visit( + [&metrics_msg, this](auto && arg) { + using T = std::decay_t; + for (const auto & [metric, value] : arg) { + if constexpr (std::is_same_v) { + if (value.count() > 0) { + metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + } + } else if constexpr (std::is_same_v) { + metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + } + } + }, + metric_result.value()); } // publish metrics @@ -111,6 +120,22 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( return status; } +DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( + const std::string & metric, const double value) const +{ + DiagnosticStatus status; + + status.level = status.OK; + status.name = metric; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(value); + status.values.push_back(key_value); + + return status; +} + void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_); diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp index 523e11883e755..eddeadc331101 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -39,7 +39,7 @@ using visualization_msgs::msg::Marker; void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { 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; @@ -57,7 +57,7 @@ void addFootprintMarker( } MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) { const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index a408821466fb9..3db487f3167ae 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -141,7 +141,19 @@ class EvalTest : public ::testing::Test [=](const DiagnosticArray::ConstSharedPtr msg) { const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); if (it != msg->status.end()) { - metric_value_ = boost::lexical_cast(it->values[2].value); + const auto mean_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "mean"; }); + if (mean_it != it->values.end()) { + metric_value_ = boost::lexical_cast(mean_it->value); + } else { + const auto metric_value_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "metric_value"; }); + if (metric_value_it != it->values.end()) { + metric_value_ = boost::lexical_cast(metric_value_it->value); + } + } metric_updated_ = true; } }); diff --git a/launch/tier4_control_launch/control_launch.drawio.svg b/launch/tier4_control_launch/control_launch.drawio.svg index 5d13b84a0b34b..21d97ba08c0b3 100644 --- a/launch/tier4_control_launch/control_launch.drawio.svg +++ b/launch/tier4_control_launch/control_launch.drawio.svg @@ -259,17 +259,17 @@ >
- lane_departure_checker_node + autoware_lane_departure_checker_node

- package: lane_departure_checker_node + package: autoware_lane_departure_checker_node
- lane_departure_checker_node... + autoware_lane_departure_checker_node... diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 77140b0e0f630..02ad160164c32 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -53,7 +53,8 @@ def launch_setup(context, *args, **kwargs): with open(LaunchConfiguration("control_validator_param_path").perform(context), "r") as f: control_validator_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open( - LaunchConfiguration("operation_mode_transition_manager_param_path").perform(context), "r" + LaunchConfiguration("operation_mode_transition_manager_param_path").perform(context), + "r", ) as f: operation_mode_transition_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] with open(LaunchConfiguration("shift_decider_param_path").perform(context), "r") as f: @@ -69,7 +70,7 @@ def launch_setup(context, *args, **kwargs): trajectory_follower_mode = LaunchConfiguration("trajectory_follower_mode").perform(context) controller_component = ComposableNode( - package="trajectory_follower_node", + package="autoware_trajectory_follower_node", plugin="autoware::motion::control::trajectory_follower_node::Controller", name="controller_node_exe", namespace="trajectory_follower", @@ -102,8 +103,8 @@ def launch_setup(context, *args, **kwargs): # lane departure checker lane_departure_component = ComposableNode( - package="lane_departure_checker", - plugin="lane_departure_checker::LaneDepartureCheckerNode", + package="autoware_lane_departure_checker", + plugin="autoware::lane_departure_checker::LaneDepartureCheckerNode", name="lane_departure_checker_node", namespace="trajectory_follower", remappings=[ @@ -122,9 +123,9 @@ def launch_setup(context, *args, **kwargs): # shift decider shift_decider_component = ComposableNode( - package="shift_decider", - plugin="ShiftDecider", - name="shift_decider", + package="autoware_shift_decider", + plugin="autoware::shift_decider::ShiftDecider", + name="autoware_shift_decider", remappings=[ ("input/control_cmd", "/control/trajectory_follower/control_cmd"), ("input/state", "/autoware/state"), @@ -139,7 +140,7 @@ def launch_setup(context, *args, **kwargs): # autonomous emergency braking autonomous_emergency_braking = ComposableNode( - package="autonomous_emergency_braking", + package="autoware_autonomous_emergency_braking", plugin="autoware::motion::control::autonomous_emergency_braking::AEB", name="autonomous_emergency_braking", remappings=[ @@ -194,8 +195,8 @@ def launch_setup(context, *args, **kwargs): # vehicle cmd gate vehicle_cmd_gate_component = ComposableNode( - package="vehicle_cmd_gate", - plugin="vehicle_cmd_gate::VehicleCmdGate", + package="autoware_vehicle_cmd_gate", + plugin="autoware::vehicle_cmd_gate::VehicleCmdGate", name="vehicle_cmd_gate", remappings=[ ("input/steering", "/vehicle/status/steering_status"), @@ -245,10 +246,10 @@ def launch_setup(context, *args, **kwargs): ) # operation mode transition manager - operation_mode_transition_manager_component = ComposableNode( - package="operation_mode_transition_manager", - plugin="operation_mode_transition_manager::OperationModeTransitionManager", - name="operation_mode_transition_manager", + autoware_operation_mode_transition_manager_component = ComposableNode( + package="autoware_operation_mode_transition_manager", + plugin="autoware::operation_mode_transition_manager::OperationModeTransitionManager", + name="autoware_operation_mode_transition_manager", remappings=[ # input ("kinematics", "/localization/kinematic_state"), @@ -272,7 +273,10 @@ def launch_setup(context, *args, **kwargs): # external cmd selector external_cmd_selector_loader = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("external_cmd_selector"), "/launch/external_cmd_selector.launch.py"] + [ + FindPackageShare("autoware_external_cmd_selector"), + "/launch/external_cmd_selector.launch.py", + ] ), launch_arguments=[ ("use_intra_process", LaunchConfiguration("use_intra_process")), @@ -287,7 +291,10 @@ def launch_setup(context, *args, **kwargs): # external cmd converter external_cmd_converter_loader = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("external_cmd_converter"), "/launch/external_cmd_converter.launch.py"] + [ + FindPackageShare("autoware_external_cmd_converter"), + "/launch/external_cmd_converter.launch.py", + ] ), launch_arguments=[ ("use_intra_process", LaunchConfiguration("use_intra_process")), @@ -341,7 +348,7 @@ def launch_setup(context, *args, **kwargs): lane_departure_component, shift_decider_component, vehicle_cmd_gate_component, - operation_mode_transition_manager_component, + autoware_operation_mode_transition_manager_component, glog_component, ], ) @@ -356,7 +363,7 @@ def launch_setup(context, *args, **kwargs): lane_departure_component, shift_decider_component, vehicle_cmd_gate_component, - operation_mode_transition_manager_component, + autoware_operation_mode_transition_manager_component, glog_component, ], ) @@ -383,8 +390,8 @@ def launch_setup(context, *args, **kwargs): # control validator checker control_validator_component = ComposableNode( - package="control_validator", - plugin="control_validator::ControlValidator", + package="autoware_control_validator", + plugin="autoware::control_validator::ControlValidator", name="control_validator", remappings=[ ("~/input/kinematics", "/localization/kinematic_state"), @@ -432,7 +439,7 @@ def launch_setup(context, *args, **kwargs): ) smart_mpc_trajectory_follower = Node( - package="smart_mpc_trajectory_follower", + package="autoware_smart_mpc_trajectory_follower", executable="pympc_trajectory_follower.py", name="pympc_trajectory_follower", ) diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 801fa274dd086..699c39f8afa63 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -11,13 +11,13 @@ ament_cmake_auto autoware_cmake + autoware_external_cmd_converter + autoware_external_cmd_selector + autoware_lane_departure_checker + autoware_shift_decider + autoware_trajectory_follower_node + autoware_vehicle_cmd_gate control_evaluator - external_cmd_converter - external_cmd_selector - lane_departure_checker - shift_decider - trajectory_follower_node - vehicle_cmd_gate ament_lint_auto autoware_lint_common diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 132d1ec9be3ea..c58c0848eb5b3 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -22,7 +22,7 @@ - + diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index 8fb41076204a8..05643e354cfce 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -4,7 +4,6 @@ tier4_map_launch 0.1.0 The tier4_map_launch package - Ryohsuke Mitsudome Ryu Yamamoto Koji Minoda Kento Yabuuchi @@ -14,6 +13,8 @@ Shintaro Sakoda Masahiro Sakamoto Apache License 2.0 + Ryohsuke Mitsudome + Yamato Ando ament_cmake_auto autoware_cmake 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 9c3e815e8abb4..7e389f5790051 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 @@ -5,7 +5,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index f941bc8d771b1..274b39be681b8 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -2,16 +2,41 @@ - + - + + + + + + + + + + + + + + + + + + + + + + + + + + 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 fa343f49840ad..a9ba980b3ce47 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 @@ -5,7 +5,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index e1a2260f3ee6d..3ce78e3b29f29 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -47,7 +47,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 21143aa0cb28a..ed7ff059a6e53 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 @@ -17,7 +17,7 @@ - + @@ -158,7 +158,7 @@ - + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 2d007ae8c5ff0..6ec706a4aad32 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -12,9 +12,10 @@ ament_cmake_auto autoware_cmake + autoware_crosswalk_traffic_light_estimator + autoware_map_based_prediction cluster_merger compare_map_segmentation - crosswalk_traffic_light_estimator detected_object_feature_remover detected_object_validation detection_by_tracker @@ -23,7 +24,6 @@ image_projection_based_fusion image_transport_decompressor lidar_apollo_instance_segmentation - map_based_prediction multi_object_tracker object_merger object_range_splitter diff --git a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml index 0885f48a6827d..cd33e7517ddc3 100644 --- a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml @@ -1,11 +1,11 @@ - + - + - + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 97bf6414189ab..1d01732454daf 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -32,7 +32,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 154a3019f1c17..9121be27825aa 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -32,7 +32,6 @@ - @@ -42,42 +41,42 @@ @@ -101,37 +100,37 @@ - - + @@ -261,7 +255,6 @@ - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 0612f67de4c2b..94f641200868e 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -2,8 +2,9 @@ - - + + + @@ -11,13 +12,18 @@ + + + + + @@ -28,7 +34,7 @@ - + @@ -62,7 +68,7 @@ - + @@ -80,7 +86,7 @@ - + @@ -98,7 +104,7 @@ - + @@ -135,43 +141,21 @@ + - - - - - - - - - - - - - - - - - - - - - - - - + - + @@ -194,7 +178,7 @@ - + @@ -219,7 +203,7 @@ - + @@ -232,7 +216,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 98315919b540a..e0fdcb1e30408 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -4,7 +4,7 @@ - + @@ -18,7 +18,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index be85ee704ff95..a252986f9d957 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -3,7 +3,7 @@ - + @@ -19,7 +19,7 @@ - + @@ -27,7 +27,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index cf96cd39043ce..03f12b43a6f85 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -57,24 +57,24 @@ ament_cmake_auto autoware_cmake + autoware_behavior_path_planner autoware_behavior_velocity_planner + autoware_costmap_generator + autoware_external_cmd_selector + autoware_external_velocity_limit_selector + autoware_freespace_planner + autoware_mission_planner + autoware_obstacle_cruise_planner autoware_path_optimizer + autoware_planning_topic_converter + autoware_planning_validator autoware_remaining_distance_time_calculator + autoware_scenario_selector + autoware_surround_obstacle_checker autoware_velocity_smoother - behavior_path_planner - costmap_generator - external_cmd_selector - external_velocity_limit_selector - freespace_planner glog_component - mission_planner - obstacle_cruise_planner obstacle_stop_planner planning_evaluator - planning_topic_converter - planning_validator - scenario_selector - surround_obstacle_checker ament_lint_auto autoware_lint_common diff --git a/launch/tier4_sensing_launch/launch/sensing.launch.xml b/launch/tier4_sensing_launch/launch/sensing.launch.xml index 391a1f8bad641..9981c99a8f3fc 100644 --- a/launch/tier4_sensing_launch/launch/sensing.launch.xml +++ b/launch/tier4_sensing_launch/launch/sensing.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/launch/tier4_sensing_launch/package.xml b/launch/tier4_sensing_launch/package.xml index a1f10ee743db5..410f685285b7d 100644 --- a/launch/tier4_sensing_launch/package.xml +++ b/launch/tier4_sensing_launch/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - vehicle_info_util + autoware_vehicle_info_utils ament_lint_auto autoware_lint_common diff --git a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml index 0e1d22bfd1827..ffd32ac92c0d5 100644 --- a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml +++ b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp b/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp index 737c25f8ce024..880ab82418e06 100644 --- a/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp @@ -22,11 +22,11 @@ template class AgedObjectQueue { public: - explicit AgedObjectQueue(const int max_age) : max_age_(max_age) {} + explicit AgedObjectQueue(const size_t max_age) : max_age_(max_age) {} - bool empty() const { return this->size() == 0; } + [[nodiscard]] bool empty() const { return this->size() == 0; } - size_t size() const { return objects_.size(); } + [[nodiscard]] size_t size() const { return objects_.size(); } Object back() const { return objects_.back(); } @@ -39,7 +39,7 @@ class AgedObjectQueue Object pop_increment_age() { const Object object = objects_.front(); - const int age = ages_.front() + 1; + const size_t age = ages_.front() + 1; objects_.pop(); ages_.pop(); @@ -54,13 +54,13 @@ class AgedObjectQueue void clear() { objects_ = std::queue(); - ages_ = std::queue(); + ages_ = std::queue(); } private: - const int max_age_; + const size_t max_age_; std::queue objects_; - std::queue ages_; + std::queue ages_; }; #endif // EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/covariance.hpp b/localization/ekf_localizer/include/ekf_localizer/covariance.hpp index a4448ecda2f45..713877c1307d2 100644 --- a/localization/ekf_localizer/include/ekf_localizer/covariance.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/covariance.hpp @@ -17,7 +17,7 @@ #include "ekf_localizer/matrix_types.hpp" -std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P); -std::array ekfCovarianceToTwistMessageCovariance(const Matrix6d & P); +std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P); +std::array ekf_covariance_to_twist_message_covariance(const Matrix6d & P); #endif // EKF_LOCALIZER__COVARIANCE_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp index f4dc6436f6a40..4c92b2947642b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp @@ -20,21 +20,21 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated); +diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( 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( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size( const std::string & measurement_type, const size_t queue_size); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate( 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( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate( 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( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( 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 8ed925c1bc228..b78e9b1ee0469 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -58,17 +58,15 @@ class Simple1DFilter x_ = 0; dev_ = 1e9; proc_dev_x_c_ = 0.0; - return; }; - void init(const double init_obs, const double obs_dev, const rclcpp::Time time) + void init(const double init_obs, const double obs_dev, const rclcpp::Time & time) { x_ = init_obs; dev_ = obs_dev; latest_time_ = time; initialized_ = true; - return; }; - void update(const double obs, const double obs_dev, const rclcpp::Time time) + void update(const double obs, const double obs_dev, const rclcpp::Time & time) { if (!initialized_) { init(obs, obs_dev, time); @@ -86,10 +84,9 @@ class Simple1DFilter dev_ = (1 - kalman_gain) * dev_; latest_time_ = time; - return; }; void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } - double get_x() const { return x_; } + [[nodiscard]] double get_x() const { return x_; } private: bool initialized_; @@ -158,10 +155,9 @@ class EKFLocalizer : public rclcpp::Node double ekf_dt_; /* process noise variance for discrete model */ - double proc_cov_yaw_d_; //!< @brief discrete yaw process noise - double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise - double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 - double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 + double proc_cov_yaw_d_; //!< @brief discrete yaw process noise + double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 + double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 bool is_activated_; @@ -174,44 +170,45 @@ class EKFLocalizer : public rclcpp::Node /** * @brief computes update & prediction of EKF for each ekf_dt_[s] time */ - void timerCallback(); + void timer_callback(); /** * @brief publish tf for tf_rate [Hz] */ - void timerTFCallback(); + void timer_tf_callback(); /** - * @brief set poseWithCovariance measurement + * @brief set pose with covariance measurement */ - void callbackPoseWithCovariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); /** - * @brief set twistWithCovariance measurement + * @brief set twist with covariance measurement */ - void callbackTwistWithCovariance(geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void callback_twist_with_covariance( + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); /** * @brief set initial_pose to current EKF pose */ - void callbackInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void callback_initial_pose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); /** * @brief update predict frequency */ - void updatePredictFrequency(const rclcpp::Time & current_time); + void update_predict_frequency(const rclcpp::Time & current_time); /** * @brief get transform from frame_id */ - bool getTransformFromTF( + bool get_transform_from_tf( std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform); /** * @brief publish current EKF estimation result */ - void publishEstimateResult( + void publish_estimate_result( const geometry_msgs::msg::PoseStamped & current_ekf_pose, const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist); @@ -219,23 +216,23 @@ class EKFLocalizer : public rclcpp::Node /** * @brief publish diagnostics message */ - void publishDiagnostics(const rclcpp::Time & current_time); + void publish_diagnostics(const rclcpp::Time & current_time); /** - * @brief update simple1DFilter + * @brief update simple 1d filter */ - void updateSimple1DFilters( + void update_simple_1d_filters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step); /** - * @brief initialize simple1DFilter + * @brief initialize simple 1d filter */ - void initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + void init_simple_1d_filters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); /** * @brief trigger node */ - void serviceTriggerNode( + void service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index e88a59ffdfab9..ee360e798f492 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -34,24 +34,13 @@ struct EKFDiagnosticInfo { - EKFDiagnosticInfo() - : no_update_count(0), - queue_size(0), - is_passed_delay_gate(true), - delay_time(0), - delay_time_threshold(0), - is_passed_mahalanobis_gate(true), - mahalanobis_distance(0) - { - } - - size_t no_update_count; - size_t queue_size; - bool is_passed_delay_gate; - double delay_time; - double delay_time_threshold; - bool is_passed_mahalanobis_gate; - double mahalanobis_distance; + size_t no_update_count{0}; + size_t queue_size{0}; + bool is_passed_delay_gate{true}; + double delay_time{0.0}; + double delay_time_threshold{0.0}; + bool is_passed_mahalanobis_gate{true}; + double mahalanobis_distance{0.0}; }; class EKFModule @@ -63,31 +52,32 @@ class EKFModule using Twist = geometry_msgs::msg::TwistStamped; public: - EKFModule(std::shared_ptr warning, const HyperParameters params); + EKFModule(std::shared_ptr warning, const HyperParameters & params); void initialize( const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform); - geometry_msgs::msg::PoseStamped getCurrentPose( + [[nodiscard]] geometry_msgs::msg::PoseStamped get_current_pose( const rclcpp::Time & current_time, const double z, const double roll, const double pitch, bool get_biased_yaw) const; - geometry_msgs::msg::TwistStamped getCurrentTwist(const rclcpp::Time & current_time) const; - double getYawBias() const; - std::array getCurrentPoseCovariance() const; - std::array getCurrentTwistCovariance() const; + [[nodiscard]] geometry_msgs::msg::TwistStamped get_current_twist( + const rclcpp::Time & current_time) const; + [[nodiscard]] double get_yaw_bias() const; + [[nodiscard]] std::array get_current_pose_covariance() const; + [[nodiscard]] std::array get_current_twist_covariance() const; - size_t find_closest_delay_time_index(double target_value) const; + [[nodiscard]] size_t find_closest_delay_time_index(double target_value) const; void accumulate_delay_time(const double dt); - void predictWithDelay(const double dt); - bool measurementUpdatePose( + void predict_with_delay(const double dt); + bool measurement_update_pose( const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info); - bool measurementUpdateTwist( + bool measurement_update_twist( const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info); - geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay( + geometry_msgs::msg::PoseWithCovarianceStamped compensate_pose_with_z_delay( const PoseWithCovariance & pose, const double delay_time); private: diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 56a13d1ecaf55..8d76102e5d64d 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -68,14 +68,14 @@ class HyperParameters const double tf_rate_; const bool publish_tf_; const bool enable_yaw_bias_estimation; - const int extend_state_step; + const size_t extend_state_step; const std::string pose_frame_id; const double pose_additional_delay; const double pose_gate_dist; - const int pose_smoothing_steps; + const size_t pose_smoothing_steps; const double twist_additional_delay; const double twist_gate_dist; - const int twist_smoothing_steps; + const size_t twist_smoothing_steps; 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 diff --git a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp index f2b9dc626e67a..1f0d75c5958d4 100644 --- a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp @@ -18,7 +18,7 @@ #include #include -double squaredMahalanobis( +double squared_mahalanobis( const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); diff --git a/localization/ekf_localizer/include/ekf_localizer/measurement.hpp b/localization/ekf_localizer/include/ekf_localizer/measurement.hpp index 4b2169f1b182f..396aaf7b9a1b4 100644 --- a/localization/ekf_localizer/include/ekf_localizer/measurement.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/measurement.hpp @@ -17,11 +17,11 @@ #include -Eigen::Matrix poseMeasurementMatrix(); -Eigen::Matrix twistMeasurementMatrix(); -Eigen::Matrix3d poseMeasurementCovariance( +Eigen::Matrix pose_measurement_matrix(); +Eigen::Matrix twist_measurement_matrix(); +Eigen::Matrix3d pose_measurement_covariance( const std::array & covariance, const size_t smoothing_step); -Eigen::Matrix2d twistMeasurementCovariance( +Eigen::Matrix2d twist_measurement_covariance( const std::array & covariance, const size_t smoothing_step); #endif // EKF_LOCALIZER__MEASUREMENT_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp index 6554f80aee014..358a2750bd3a8 100644 --- a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp @@ -19,12 +19,12 @@ #include -inline bool hasInf(const Eigen::MatrixXd & v) +inline bool has_inf(const Eigen::MatrixXd & v) { return v.array().isInf().any(); } -inline bool hasNan(const Eigen::MatrixXd & v) +inline bool has_nan(const Eigen::MatrixXd & v) { return v.array().isNaN().any(); } diff --git a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp index 85a65828e4ee4..09a87e5fe154b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp @@ -17,10 +17,10 @@ #include "ekf_localizer/matrix_types.hpp" -double normalizeYaw(const double & yaw); -Vector6d predictNextState(const Vector6d & X_curr, const double dt); -Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt); -Matrix6d processNoiseCovariance( +double normalize_yaw(const double & yaw); +Vector6d predict_next_state(const Vector6d & X_curr, const double dt); +Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt); +Matrix6d process_noise_covariance( const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d); #endif // EKF_LOCALIZER__STATE_TRANSITION_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/string.hpp b/localization/ekf_localizer/include/ekf_localizer/string.hpp index a4cd1f320367c..0154e84b88004 100644 --- a/localization/ekf_localizer/include/ekf_localizer/string.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/string.hpp @@ -17,7 +17,7 @@ #include -inline std::string eraseLeadingSlash(const std::string & s) +inline std::string erase_leading_slash(const std::string & s) { std::string a = s; if (a.front() == '/') { diff --git a/localization/ekf_localizer/include/ekf_localizer/warning.hpp b/localization/ekf_localizer/include/ekf_localizer/warning.hpp index a3c8800242e2b..e7456d73ecfdd 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning.hpp @@ -29,7 +29,7 @@ class Warning RCLCPP_WARN(node_->get_logger(), "%s", message.c_str()); } - void warnThrottle(const std::string & message, const int duration_milliseconds) const + void warn_throttle(const std::string & message, const int duration_milliseconds) const { RCLCPP_WARN_THROTTLE( node_->get_logger(), *(node_->get_clock()), diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index e1eafdc6f7948..36c0eabd588fa 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,11 +17,12 @@ #include -std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold); -std::string twistDelayStepWarningMessage( +std::string pose_delay_step_warning_message( const double delay_time, const double delay_time_threshold); -std::string poseDelayTimeWarningMessage(const double delay_time); -std::string twistDelayTimeWarningMessage(const double delay_time); -std::string mahalanobisWarningMessage(const double distance, const double max_distance); +std::string twist_delay_step_warning_message( + const double delay_time, const double delay_time_threshold); +std::string pose_delay_time_warning_message(const double delay_time); +std::string twist_delay_time_warning_message(const double delay_time); +std::string mahalanobis_warning_message(const double distance, const double max_distance); #endif // EKF_LOCALIZER__WARNING_MESSAGE_HPP_ diff --git a/localization/ekf_localizer/src/covariance.cpp b/localization/ekf_localizer/src/covariance.cpp index 0c98ac6857ea5..fc16abf429d93 100644 --- a/localization/ekf_localizer/src/covariance.cpp +++ b/localization/ekf_localizer/src/covariance.cpp @@ -19,9 +19,9 @@ using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; -std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P) +std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P) { - std::array covariance; + std::array covariance{}; covariance.fill(0.); covariance[COV_IDX::X_X] = P(IDX::X, IDX::X); @@ -37,9 +37,9 @@ std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P) return covariance; } -std::array ekfCovarianceToTwistMessageCovariance(const Matrix6d & P) +std::array ekf_covariance_to_twist_message_covariance(const Matrix6d & P) { - std::array covariance; + std::array covariance{}; covariance.fill(0.); covariance[COV_IDX::X_X] = P(IDX::VX, IDX::VX); diff --git a/localization/ekf_localizer/src/diagnostics.cpp b/localization/ekf_localizer/src/diagnostics.cpp index 9ff36561abaa9..ecbeaf8b2445a 100644 --- a/localization/ekf_localizer/src/diagnostics.cpp +++ b/localization/ekf_localizer/src/diagnostics.cpp @@ -19,7 +19,7 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated) +diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -38,7 +38,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activ return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( 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) { @@ -69,7 +69,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size( const std::string & measurement_type, const size_t queue_size) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -85,7 +85,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate( const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, const double delay_time_threshold) { @@ -112,7 +112,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate( const std::string & measurement_type, const bool is_passed_mahalanobis_gate, const double mahalanobis_distance, const double mahalanobis_distance_threshold) { @@ -141,7 +141,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( // 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( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 687e812679574..8e7121ec73a12 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -34,8 +34,8 @@ #include // clang-format off -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} +#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl // NOLINT +#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} // NOLINT // clang-format on using std::placeholders::_1; @@ -58,12 +58,12 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) /* initialize ros system */ timer_control_ = rclcpp::create_timer( this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), - std::bind(&EKFLocalizer::timerCallback, this)); + std::bind(&EKFLocalizer::timer_callback, this)); if (params_.publish_tf_) { timer_tf_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), - std::bind(&EKFLocalizer::timerTFCallback, this)); + std::bind(&EKFLocalizer::timer_tf_callback, this)); } pub_pose_ = create_publisher("ekf_pose", 1); @@ -79,15 +79,17 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) "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)); + "initialpose", 1, std::bind(&EKFLocalizer::callback_initial_pose, this, _1)); sub_pose_with_cov_ = create_subscription( - "in_pose_with_covariance", 1, std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, _1)); + "in_pose_with_covariance", 1, + std::bind(&EKFLocalizer::callback_pose_with_covariance, this, _1)); sub_twist_with_cov_ = create_subscription( - "in_twist_with_covariance", 1, std::bind(&EKFLocalizer::callbackTwistWithCovariance, this, _1)); + "in_twist_with_covariance", 1, + std::bind(&EKFLocalizer::callback_twist_with_covariance, this, _1)); service_trigger_node_ = create_service( "trigger_node_srv", std::bind( - &EKFLocalizer::serviceTriggerNode, this, std::placeholders::_1, std::placeholders::_2), + &EKFLocalizer::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile()); tf_br_ = std::make_shared( @@ -102,9 +104,9 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) } /* - * updatePredictFrequency + * update_predict_frequency */ -void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) +void EKFLocalizer::update_predict_frequency(const rclcpp::Time & current_time) { if (last_predict_time_) { if (current_time < *last_predict_time_) { @@ -119,7 +121,7 @@ void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) ekf_dt_ = 10.0; RCLCPP_WARN( get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_); - } else if (ekf_dt_ > params_.pose_smoothing_steps / params_.ekf_rate) { + } else if (ekf_dt_ > static_cast(params_.pose_smoothing_steps) / params_.ekf_rate) { RCLCPP_WARN( get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_); } @@ -137,28 +139,28 @@ void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) } /* - * timerCallback + * timer_callback */ -void EKFLocalizer::timerCallback() +void EKFLocalizer::timer_callback() { const rclcpp::Time current_time = this->now(); if (!is_activated_) { - warning_->warnThrottle( + warning_->warn_throttle( "The node is not activated. Provide initial pose to pose_initializer", 2000); - publishDiagnostics(current_time); + publish_diagnostics(current_time); return; } DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ - updatePredictFrequency(current_time); + update_predict_frequency(current_time); /* predict model in EKF */ stop_watch_.tic(); DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------"); - ekf_module_->predictWithDelay(ekf_dt_); + ekf_module_->predict_with_delay(ekf_dt_); DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); @@ -177,22 +179,22 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = current_time; const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdatePose(*pose, t_curr, pose_diag_info_); + bool is_updated = ekf_module_->measurement_update_pose(*pose, current_time, pose_diag_info_); if (is_updated) { pose_is_updated = true; // Update Simple 1D filter with considering change of z value due to measurement pose delay const double delay_time = - (t_curr - pose->header.stamp).seconds() + params_.pose_additional_delay; - const auto pose_with_z_delay = ekf_module_->compensatePoseWithZDelay(*pose, delay_time); - updateSimple1DFilters(pose_with_z_delay, params_.pose_smoothing_steps); + (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay; + const auto pose_with_z_delay = ekf_module_->compensate_pose_with_z_delay(*pose, delay_time); + update_simple_1d_filters(pose_with_z_delay, params_.pose_smoothing_steps); } } - DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO( + get_logger(), "[EKF] measurement_update_pose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1); @@ -212,16 +214,17 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = current_time; const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdateTwist(*twist, t_curr, twist_diag_info_); + bool is_updated = + ekf_module_->measurement_update_twist(*twist, current_time, twist_diag_info_); if (is_updated) { twist_is_updated = true; } } - DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO( + get_logger(), "[EKF] measurement_update_twist calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); } twist_diag_info_.no_update_count = twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1); @@ -230,27 +233,27 @@ void EKFLocalizer::timerCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); const geometry_msgs::msg::PoseStamped current_ekf_pose = - ekf_module_->getCurrentPose(current_time, z, roll, pitch, false); + ekf_module_->get_current_pose(current_time, z, roll, pitch, false); const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = - ekf_module_->getCurrentPose(current_time, z, roll, pitch, true); + ekf_module_->get_current_pose(current_time, z, roll, pitch, true); const geometry_msgs::msg::TwistStamped current_ekf_twist = - ekf_module_->getCurrentTwist(current_time); + ekf_module_->get_current_twist(current_time); /* publish ekf result */ - publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); - publishDiagnostics(current_time); + publish_estimate_result(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); + publish_diagnostics(current_time); } /* - * timerTFCallback + * timer_tf_callback */ -void EKFLocalizer::timerTFCallback() +void EKFLocalizer::timer_tf_callback() { if (!is_activated_) { return; } - if (params_.pose_frame_id == "") { + if (params_.pose_frame_id.empty()) { return; } @@ -262,15 +265,15 @@ void EKFLocalizer::timerTFCallback() geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped = tier4_autoware_utils::pose2transform( - ekf_module_->getCurrentPose(current_time, z, roll, pitch, false), "base_link"); + ekf_module_->get_current_pose(current_time, z, roll, pitch, false), "base_link"); transform_stamped.header.stamp = current_time; tf_br_->sendTransform(transform_stamped); } /* - * getTransformFromTF + * get_transform_from_tf */ -bool EKFLocalizer::getTransformFromTF( +bool EKFLocalizer::get_transform_from_tf( std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform) { @@ -278,8 +281,8 @@ bool EKFLocalizer::getTransformFromTF( tf2_ros::TransformListener tf_listener(tf_buffer); rclcpp::sleep_for(std::chrono::milliseconds(100)); - parent_frame = eraseLeadingSlash(parent_frame); - child_frame = eraseLeadingSlash(child_frame); + parent_frame = erase_leading_slash(parent_frame); + child_frame = erase_leading_slash(child_frame); for (int i = 0; i < 50; ++i) { try { @@ -294,25 +297,25 @@ bool EKFLocalizer::getTransformFromTF( } /* - * callbackInitialPose + * callback_initial_pose */ -void EKFLocalizer::callbackInitialPose( - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initialpose) +void EKFLocalizer::callback_initial_pose( + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { geometry_msgs::msg::TransformStamped transform; - if (!getTransformFromTF(params_.pose_frame_id, initialpose->header.frame_id, transform)) { + if (!get_transform_from_tf(params_.pose_frame_id, msg->header.frame_id, transform)) { RCLCPP_ERROR( get_logger(), "[EKF] TF transform failed. parent = %s, child = %s", - params_.pose_frame_id.c_str(), initialpose->header.frame_id.c_str()); + params_.pose_frame_id.c_str(), msg->header.frame_id.c_str()); } - ekf_module_->initialize(*initialpose, transform); - initSimple1DFilters(*initialpose); + ekf_module_->initialize(*msg, transform); + init_simple_1d_filters(*msg); } /* - * callbackPoseWithCovariance + * callback_pose_with_covariance */ -void EKFLocalizer::callbackPoseWithCovariance( +void EKFLocalizer::callback_pose_with_covariance( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { if (!is_activated_) { @@ -323,9 +326,9 @@ void EKFLocalizer::callbackPoseWithCovariance( } /* - * callbackTwistWithCovariance + * callback_twist_with_covariance */ -void EKFLocalizer::callbackTwistWithCovariance( +void EKFLocalizer::callback_twist_with_covariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { // Ignore twist if velocity is too small. @@ -337,9 +340,9 @@ void EKFLocalizer::callbackTwistWithCovariance( } /* - * publishEstimateResult + * publish_estimate_result */ -void EKFLocalizer::publishEstimateResult( +void EKFLocalizer::publish_estimate_result( const geometry_msgs::msg::PoseStamped & current_ekf_pose, const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist) @@ -353,7 +356,7 @@ void EKFLocalizer::publishEstimateResult( pose_cov.header.stamp = current_ekf_pose.header.stamp; pose_cov.header.frame_id = current_ekf_pose.header.frame_id; pose_cov.pose.pose = current_ekf_pose.pose; - pose_cov.pose.covariance = ekf_module_->getCurrentPoseCovariance(); + pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance(); pub_pose_cov_->publish(pose_cov); geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; @@ -368,13 +371,13 @@ void EKFLocalizer::publishEstimateResult( twist_cov.header.stamp = current_ekf_twist.header.stamp; twist_cov.header.frame_id = current_ekf_twist.header.frame_id; twist_cov.twist.twist = current_ekf_twist.twist; - twist_cov.twist.covariance = ekf_module_->getCurrentTwistCovariance(); + twist_cov.twist.covariance = ekf_module_->get_current_twist_covariance(); pub_twist_cov_->publish(twist_cov); /* publish yaw bias */ tier4_debug_msgs::msg::Float64Stamped yawb; yawb.stamp = current_ekf_twist.header.stamp; - yawb.data = ekf_module_->getYawBias(); + yawb.data = ekf_module_->get_yaw_bias(); pub_yaw_bias_->publish(yawb); /* publish latest odometry */ @@ -387,38 +390,38 @@ void EKFLocalizer::publishEstimateResult( pub_odom_->publish(odometry); } -void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) +void EKFLocalizer::publish_diagnostics(const rclcpp::Time & current_time) { std::vector diag_status_array; - diag_status_array.push_back(checkProcessActivated(is_activated_)); + diag_status_array.push_back(check_process_activated(is_activated_)); if (is_activated_) { - diag_status_array.push_back(checkMeasurementUpdated( + diag_status_array.push_back(check_measurement_updated( "pose", pose_diag_info_.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_diag_info_.queue_size)); - diag_status_array.push_back(checkMeasurementDelayGate( + diag_status_array.push_back(check_measurement_queue_size("pose", pose_diag_info_.queue_size)); + diag_status_array.push_back(check_measurement_delay_gate( "pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time, pose_diag_info_.delay_time_threshold)); - diag_status_array.push_back(checkMeasurementMahalanobisGate( + diag_status_array.push_back(check_measurement_mahalanobis_gate( "pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance, params_.pose_gate_dist)); - diag_status_array.push_back(checkMeasurementUpdated( + diag_status_array.push_back(check_measurement_updated( "twist", twist_diag_info_.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_diag_info_.queue_size)); - diag_status_array.push_back(checkMeasurementDelayGate( + diag_status_array.push_back(check_measurement_queue_size("twist", twist_diag_info_.queue_size)); + diag_status_array.push_back(check_measurement_delay_gate( "twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time, twist_diag_info_.delay_time_threshold)); - diag_status_array.push_back(checkMeasurementMahalanobisGate( + diag_status_array.push_back(check_measurement_mahalanobis_gate( "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance, params_.twist_gate_dist)); } diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; - diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status = merge_diagnostic_status(diag_status_array); diag_merged_status.name = "localization: " + std::string(this->get_name()); diag_merged_status.hardware_id = this->get_name(); @@ -428,7 +431,7 @@ void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) pub_diag_->publish(diag_msg); } -void EKFLocalizer::updateSimple1DFilters( +void EKFLocalizer::update_simple_1d_filters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) { double z = pose.pose.pose.position.z; @@ -446,7 +449,8 @@ void EKFLocalizer::updateSimple1DFilters( pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp); } -void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void EKFLocalizer::init_simple_1d_filters( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { double z = pose.pose.pose.position.z; @@ -465,7 +469,7 @@ void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovaria /** * @brief trigger node */ -void EKFLocalizer::serviceTriggerNode( +void EKFLocalizer::service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) { @@ -477,7 +481,6 @@ void EKFLocalizer::serviceTriggerNode( is_activated_ = false; } res->success = true; - return; } #include diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 8977d82f34138..8703d8754eaaf 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -30,58 +30,56 @@ #include // clang-format off -#define DEBUG_PRINT_MAT(X) {\ - if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}\ -} +#define DEBUG_PRINT_MAT(X) {if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}} // NOLINT // clang-format on -EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters params) +EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & params) : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz accumulated_delay_times_(params.extend_state_step, 1.0E15), params_(params) { - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y - P(IDX::YAW, IDX::YAW) = 50.0; // for yaw + Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1); + Eigen::MatrixXd p = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y + p(IDX::YAW, IDX::YAW) = 50.0; // for yaw if (params_.enable_yaw_bias_estimation) { - P(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias + p(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias } - P(IDX::VX, IDX::VX) = 1000.0; // for vx - P(IDX::WZ, IDX::WZ) = 50.0; // for wz + p(IDX::VX, IDX::VX) = 1000.0; // for vx + p(IDX::WZ, IDX::WZ) = 50.0; // for wz - kalman_filter_.init(X, P, params_.extend_state_step); + kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); } void EKFModule::initialize( const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform) { - Eigen::MatrixXd X(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); + Eigen::MatrixXd x(dim_x_, 1); + Eigen::MatrixXd p = Eigen::MatrixXd::Zero(dim_x_, dim_x_); - X(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x; - X(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y; - X(IDX::YAW) = + x(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x; + x(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y; + x(IDX::YAW) = tf2::getYaw(initial_pose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); - X(IDX::YAWB) = 0.0; - X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; + x(IDX::YAWB) = 0.0; + x(IDX::VX) = 0.0; + x(IDX::WZ) = 0.0; using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - P(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; - P(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; - P(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; + p(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; + p(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; + p(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; if (params_.enable_yaw_bias_estimation) { - P(IDX::YAWB, IDX::YAWB) = 0.0001; + p(IDX::YAWB, IDX::YAWB) = 0.0001; } - P(IDX::VX, IDX::VX) = 0.01; - P(IDX::WZ, IDX::WZ) = 0.01; + p(IDX::VX, IDX::VX) = 0.01; + p(IDX::WZ, IDX::WZ) = 0.01; - kalman_filter_.init(X, P, params_.extend_state_step); + kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); } -geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( +geometry_msgs::msg::PoseStamped EKFModule::get_current_pose( const rclcpp::Time & current_time, const double z, const double roll, const double pitch, bool get_biased_yaw) const { @@ -110,7 +108,8 @@ geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( return current_ekf_pose; } -geometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(const rclcpp::Time & current_time) const +geometry_msgs::msg::TwistStamped EKFModule::get_current_twist( + const rclcpp::Time & current_time) const { const double vx = kalman_filter_.getXelement(IDX::VX); const double wz = kalman_filter_.getXelement(IDX::WZ); @@ -123,17 +122,17 @@ geometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(const rclcpp::Time & return current_ekf_twist; } -std::array EKFModule::getCurrentPoseCovariance() const +std::array EKFModule::get_current_pose_covariance() const { - return ekfCovarianceToPoseMessageCovariance(kalman_filter_.getLatestP()); + return ekf_covariance_to_pose_message_covariance(kalman_filter_.getLatestP()); } -std::array EKFModule::getCurrentTwistCovariance() const +std::array EKFModule::get_current_twist_covariance() const { - return ekfCovarianceToTwistMessageCovariance(kalman_filter_.getLatestP()); + return ekf_covariance_to_twist_message_covariance(kalman_filter_.getLatestP()); } -double EKFModule::getYawBias() const +double EKFModule::get_yaw_bias() const { return kalman_filter_.getLatestX()(IDX::YAWB); } @@ -153,17 +152,17 @@ size_t EKFModule::find_closest_delay_time_index(double target_value) const // If else, take the closest element. if (lower == accumulated_delay_times_.begin()) { return 0; - } else if (lower == accumulated_delay_times_.end()) { + } + if (lower == accumulated_delay_times_.end()) { return accumulated_delay_times_.size() - 1; - } else { - // Compare the target with the lower bound and the previous element. - auto prev = lower - 1; - bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); - - // Return the index of the closer element. - return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) - : std::distance(accumulated_delay_times_.begin(), lower); } + // Compare the target with the lower bound and the previous element. + auto prev = lower - 1; + bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); + + // Return the index of the closer element. + return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) + : std::distance(accumulated_delay_times_.begin(), lower); } void EKFModule::accumulate_delay_time(const double dt) @@ -180,69 +179,70 @@ void EKFModule::accumulate_delay_time(const double dt) } } -void EKFModule::predictWithDelay(const double dt) +void EKFModule::predict_with_delay(const double dt) { - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); const double proc_cov_vx_d = std::pow(params_.proc_stddev_vx_c * dt, 2.0); const double proc_cov_wz_d = std::pow(params_.proc_stddev_wz_c * dt, 2.0); const double proc_cov_yaw_d = std::pow(params_.proc_stddev_yaw_c * dt, 2.0); - const Vector6d X_next = predictNextState(X_curr, dt); - const Matrix6d A = createStateTransitionMatrix(X_curr, dt); - const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); - kalman_filter_.predictWithDelay(X_next, A, Q); + const Vector6d x_next = predict_next_state(x_curr, dt); + const Matrix6d a = create_state_transition_matrix(x_curr, dt); + const Matrix6d q = process_noise_covariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); + kalman_filter_.predictWithDelay(x_next, a, q); } -bool EKFModule::measurementUpdatePose( +bool EKFModule::measurement_update_pose( const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info) { if (pose.header.frame_id != params_.pose_frame_id) { - warning_->warnThrottle( + warning_->warn_throttle( fmt::format( "pose frame_id is %s, but pose_frame is set as %s. They must be same.", pose.header.frame_id.c_str(), params_.pose_frame_id.c_str()), 2000); } - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_curr.transpose()); constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output /* Calculate delay step */ double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; if (delay_time < 0.0) { - warning_->warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000); + warning_->warn_throttle(pose_delay_time_warning_message(delay_time), 1000); } delay_time = std::max(delay_time, 0.0); - const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); + const size_t delay_step = find_closest_delay_time_index(delay_time); pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time); pose_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { pose_diag_info.is_passed_delay_gate = false; - warning_->warnThrottle( - poseDelayStepWarningMessage(pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), + warning_->warn_throttle( + pose_delay_step_warning_message( + pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), 2000); return false; } /* Since the kalman filter cannot handle the rotation angle directly, - offset the yaw angle so that the difference from the yaw angle that ekf holds internally is less - than 2 pi. */ + offset the yaw angle so that the difference from the yaw angle that ekf holds internally + is less than 2 pi. */ double yaw = tf2::getYaw(pose.pose.pose.orientation); const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); - const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi + const double yaw_error = normalize_yaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi yaw = yaw_error + ekf_yaw; /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw; - if (hasNan(y) || hasInf(y)) { + if (has_nan(y) || has_inf(y)) { warning_->warn( "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); return false; @@ -252,15 +252,15 @@ bool EKFModule::measurementUpdatePose( const Eigen::Vector3d y_ekf( kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X), kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); - const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd p_y = p_curr.block(0, 0, dim_y, dim_y); - const double distance = mahalanobis(y_ekf, y, P_y); + const double distance = mahalanobis(y_ekf, y, p_y); pose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance); if (distance > params_.pose_gate_dist) { pose_diag_info.is_passed_mahalanobis_gate = false; - warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); - warning_->warnThrottle("Ignore the measurement data.", 2000); + warning_->warn_throttle(mahalanobis_warning_message(distance, params_.pose_gate_dist), 2000); + warning_->warn_throttle("Ignore the measurement data.", 2000); return false; } @@ -268,21 +268,21 @@ bool EKFModule::measurementUpdatePose( DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); - const Eigen::Matrix C = poseMeasurementMatrix(); - const Eigen::Matrix3d R = - poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps); + const Eigen::Matrix c = pose_measurement_matrix(); + const Eigen::Matrix3d r = + pose_measurement_covariance(pose.pose.covariance, params_.pose_smoothing_steps); - kalman_filter_.updateWithDelay(y, C, R, delay_step); + kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); // debug - const Eigen::MatrixXd X_result = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_result.transpose()); + DEBUG_PRINT_MAT((x_result - x_curr).transpose()); return true; } -geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDelay( +geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay( const PoseWithCovariance & pose, const double delay_time) { const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); @@ -293,34 +293,34 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDela return pose_with_z_delay; } -bool EKFModule::measurementUpdateTwist( +bool EKFModule::measurement_update_twist( const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info) { if (twist.header.frame_id != "base_link") { - warning_->warnThrottle("twist frame_id must be base_link", 2000); + warning_->warn_throttle("twist frame_id must be base_link", 2000); } - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_curr.transpose()); constexpr int dim_y = 2; // vx, wz /* Calculate delay step */ double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay; if (delay_time < 0.0) { - warning_->warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000); + warning_->warn_throttle(twist_delay_time_warning_message(delay_time), 1000); } delay_time = std::max(delay_time, 0.0); - const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); + const size_t delay_step = find_closest_delay_time_index(delay_time); twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time); twist_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { twist_diag_info.is_passed_delay_gate = false; - warning_->warnThrottle( - twistDelayStepWarningMessage( + warning_->warn_throttle( + twist_delay_step_warning_message( twist_diag_info.delay_time, twist_diag_info.delay_time_threshold), 2000); return false; @@ -330,7 +330,7 @@ bool EKFModule::measurementUpdateTwist( Eigen::MatrixXd y(dim_y, 1); y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; - if (hasNan(y) || hasInf(y)) { + if (has_nan(y) || has_inf(y)) { warning_->warn( "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); return false; @@ -339,15 +339,15 @@ bool EKFModule::measurementUpdateTwist( const Eigen::Vector2d y_ekf( kalman_filter_.getXelement(delay_step * dim_x_ + IDX::VX), kalman_filter_.getXelement(delay_step * dim_x_ + IDX::WZ)); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); - const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd p_y = p_curr.block(4, 4, dim_y, dim_y); - const double distance = mahalanobis(y_ekf, y, P_y); + const double distance = mahalanobis(y_ekf, y, p_y); twist_diag_info.mahalanobis_distance = std::max(distance, twist_diag_info.mahalanobis_distance); if (distance > params_.twist_gate_dist) { twist_diag_info.is_passed_mahalanobis_gate = false; - warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); - warning_->warnThrottle("Ignore the measurement data.", 2000); + warning_->warn_throttle(mahalanobis_warning_message(distance, params_.twist_gate_dist), 2000); + warning_->warn_throttle("Ignore the measurement data.", 2000); return false; } @@ -355,16 +355,16 @@ bool EKFModule::measurementUpdateTwist( DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); - const Eigen::Matrix C = twistMeasurementMatrix(); - const Eigen::Matrix2d R = - twistMeasurementCovariance(twist.twist.covariance, params_.twist_smoothing_steps); + const Eigen::Matrix c = twist_measurement_matrix(); + const Eigen::Matrix2d r = + twist_measurement_covariance(twist.twist.covariance, params_.twist_smoothing_steps); - kalman_filter_.updateWithDelay(y, C, R, delay_step); + kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); // debug - const Eigen::MatrixXd X_result = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_result.transpose()); + DEBUG_PRINT_MAT((x_result - x_curr).transpose()); return true; } diff --git a/localization/ekf_localizer/src/mahalanobis.cpp b/localization/ekf_localizer/src/mahalanobis.cpp index ff5ef13892b73..a71482ab696f6 100644 --- a/localization/ekf_localizer/src/mahalanobis.cpp +++ b/localization/ekf_localizer/src/mahalanobis.cpp @@ -14,7 +14,7 @@ #include "ekf_localizer/mahalanobis.hpp" -double squaredMahalanobis( +double squared_mahalanobis( const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) { const Eigen::VectorXd d = x - y; @@ -23,5 +23,5 @@ double squaredMahalanobis( double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) { - return std::sqrt(squaredMahalanobis(x, y, C)); + return std::sqrt(squared_mahalanobis(x, y, C)); } diff --git a/localization/ekf_localizer/src/measurement.cpp b/localization/ekf_localizer/src/measurement.cpp index 0b3e65bd60f5d..4533bacbee991 100644 --- a/localization/ekf_localizer/src/measurement.cpp +++ b/localization/ekf_localizer/src/measurement.cpp @@ -17,40 +17,40 @@ #include "ekf_localizer/state_index.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" -Eigen::Matrix poseMeasurementMatrix() +Eigen::Matrix pose_measurement_matrix() { - Eigen::Matrix C = Eigen::Matrix::Zero(); - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - return C; + Eigen::Matrix c = Eigen::Matrix::Zero(); + c(0, IDX::X) = 1.0; // for pos x + c(1, IDX::Y) = 1.0; // for pos y + c(2, IDX::YAW) = 1.0; // for yaw + return c; } -Eigen::Matrix twistMeasurementMatrix() +Eigen::Matrix twist_measurement_matrix() { - Eigen::Matrix C = Eigen::Matrix::Zero(); - C(0, IDX::VX) = 1.0; // for vx - C(1, IDX::WZ) = 1.0; // for wz - return C; + Eigen::Matrix c = Eigen::Matrix::Zero(); + c(0, IDX::VX) = 1.0; // for vx + c(1, IDX::WZ) = 1.0; // for wz + return c; } -Eigen::Matrix3d poseMeasurementCovariance( +Eigen::Matrix3d pose_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { - Eigen::Matrix3d R; + Eigen::Matrix3d r; using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - R << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), + r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::Y_X), covariance.at(COV_IDX::Y_Y), covariance.at(COV_IDX::Y_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_Y), covariance.at(COV_IDX::YAW_YAW); - return R * static_cast(smoothing_step); + return r * static_cast(smoothing_step); } -Eigen::Matrix2d twistMeasurementCovariance( +Eigen::Matrix2d twist_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { - Eigen::Matrix2d R; + Eigen::Matrix2d r; using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - R << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), + r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_YAW); - return R * static_cast(smoothing_step); + return r * static_cast(smoothing_step); } diff --git a/localization/ekf_localizer/src/state_transition.cpp b/localization/ekf_localizer/src/state_transition.cpp index 88fc9f76d7168..22b1f2f8a1c67 100644 --- a/localization/ekf_localizer/src/state_transition.cpp +++ b/localization/ekf_localizer/src/state_transition.cpp @@ -17,7 +17,7 @@ #include -double normalizeYaw(const double & yaw) +double normalize_yaw(const double & yaw) { // FIXME(IshitaTakeshi) I think the computation here can be simplified // FIXME(IshitaTakeshi) Rename the function. This is not normalization @@ -36,7 +36,7 @@ double normalizeYaw(const double & yaw) * (b_k : yaw_bias_k) */ -Vector6d predictNextState(const Vector6d & X_curr, const double dt) +Vector6d predict_next_state(const Vector6d & X_curr, const double dt) { const double x = X_curr(IDX::X); const double y = X_curr(IDX::Y); @@ -45,14 +45,14 @@ Vector6d predictNextState(const Vector6d & X_curr, const double dt) const double vx = X_curr(IDX::VX); const double wz = X_curr(IDX::WZ); - Vector6d X_next; - X_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) - X_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) - X_next(IDX::YAW) = normalizeYaw(yaw + wz * dt); // dyaw = omega + omega_bias - X_next(IDX::YAWB) = yaw_bias; - X_next(IDX::VX) = vx; - X_next(IDX::WZ) = wz; - return X_next; + Vector6d x_next; + x_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) + x_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) + x_next(IDX::YAW) = normalize_yaw(yaw + wz * dt); // dyaw = omega + omega_bias + x_next(IDX::YAWB) = yaw_bias; + x_next(IDX::VX) = vx; + x_next(IDX::WZ) = wz; + return x_next; } /* == Linearized model == @@ -64,32 +64,32 @@ Vector6d predictNextState(const Vector6d & X_curr, const double dt) * [ 0, 0, 0, 0, 1, 0] * [ 0, 0, 0, 0, 0, 1] */ -Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt) +Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt) { const double yaw = X_curr(IDX::YAW); const double yaw_bias = X_curr(IDX::YAWB); const double vx = X_curr(IDX::VX); - Matrix6d A = Matrix6d::Identity(); - A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; - A(IDX::YAW, IDX::WZ) = dt; - return A; + Matrix6d a = Matrix6d::Identity(); + a(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; + a(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; + a(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; + a(IDX::YAW, IDX::WZ) = dt; + return a; } -Matrix6d processNoiseCovariance( +Matrix6d process_noise_covariance( const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d) { - Matrix6d Q = Matrix6d::Zero(); - Q(IDX::X, IDX::X) = 0.0; - Q(IDX::Y, IDX::Y) = 0.0; - Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw - Q(IDX::YAWB, IDX::YAWB) = 0.0; - Q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx - Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz - return Q; + Matrix6d q = Matrix6d::Zero(); + q(IDX::X, IDX::X) = 0.0; + q(IDX::Y, IDX::Y) = 0.0; + q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw + q(IDX::YAWB, IDX::YAWB) = 0.0; + q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx + q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz + return q; } diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index c69f30b2d6601..ae74c6bfb5fbc 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -18,7 +18,8 @@ #include -std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) +std::string pose_delay_step_warning_message( + const double delay_time, const double delay_time_threshold) { const std::string s = "Pose delay exceeds the compensation limit, ignored. " @@ -26,7 +27,8 @@ std::string poseDelayStepWarningMessage(const double delay_time, const double de return fmt::format(s, delay_time, delay_time_threshold); } -std::string twistDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) +std::string twist_delay_step_warning_message( + const double delay_time, const double delay_time_threshold) { const std::string s = "Twist delay exceeds the compensation limit, ignored. " @@ -34,19 +36,19 @@ std::string twistDelayStepWarningMessage(const double delay_time, const double d return fmt::format(s, delay_time, delay_time_threshold); } -std::string poseDelayTimeWarningMessage(const double delay_time) +std::string pose_delay_time_warning_message(const double delay_time) { const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; return fmt::format(s, delay_time); } -std::string twistDelayTimeWarningMessage(const double delay_time) +std::string twist_delay_time_warning_message(const double delay_time) { const std::string s = "Twist time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; return fmt::format(s, delay_time); } -std::string mahalanobisWarningMessage(const double distance, const double max_distance) +std::string mahalanobis_warning_message(const double distance, const double max_distance) { const std::string s = "The Mahalanobis distance {:.4f} is over the limit {:.4f}."; return fmt::format(s, distance, max_distance); diff --git a/localization/ekf_localizer/test/test_covariance.cpp b/localization/ekf_localizer/test/test_covariance.cpp index 23458fb18a838..ed87ebdea029b 100644 --- a/localization/ekf_localizer/test/test_covariance.cpp +++ b/localization/ekf_localizer/test/test_covariance.cpp @@ -19,18 +19,18 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) { { - Matrix6d P = Matrix6d::Zero(); - P(0, 0) = 1.; - P(0, 1) = 2.; - P(0, 2) = 3.; - P(1, 0) = 4.; - P(1, 1) = 5.; - P(1, 2) = 6.; - P(2, 0) = 7.; - P(2, 1) = 8.; - P(2, 2) = 9.; + Matrix6d p = Matrix6d::Zero(); + p(0, 0) = 1.; + p(0, 1) = 2.; + p(0, 2) = 3.; + p(1, 0) = 4.; + p(1, 1) = 5.; + p(1, 2) = 6.; + p(2, 0) = 7.; + p(2, 1) = 8.; + p(2, 2) = 9.; - std::array covariance = ekfCovarianceToPoseMessageCovariance(P); + std::array covariance = ekf_covariance_to_pose_message_covariance(p); EXPECT_EQ(covariance[0], 1.); EXPECT_EQ(covariance[1], 2.); EXPECT_EQ(covariance[5], 3.); @@ -44,8 +44,8 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) // ensure other elements are zero { - Matrix6d P = Matrix6d::Zero(); - std::array covariance = ekfCovarianceToPoseMessageCovariance(P); + Matrix6d p = Matrix6d::Zero(); + std::array covariance = ekf_covariance_to_pose_message_covariance(p); for (double e : covariance) { EXPECT_EQ(e, 0.); } @@ -55,13 +55,13 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) { { - Matrix6d P = Matrix6d::Zero(); - P(4, 4) = 1.; - P(4, 5) = 2.; - P(5, 4) = 3.; - P(5, 5) = 4.; + Matrix6d p = Matrix6d::Zero(); + p(4, 4) = 1.; + p(4, 5) = 2.; + p(5, 4) = 3.; + p(5, 5) = 4.; - std::array covariance = ekfCovarianceToTwistMessageCovariance(P); + std::array covariance = ekf_covariance_to_twist_message_covariance(p); EXPECT_EQ(covariance[0], 1.); EXPECT_EQ(covariance[5], 2.); EXPECT_EQ(covariance[30], 3.); @@ -70,8 +70,8 @@ TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) // ensure other elements are zero { - Matrix6d P = Matrix6d::Zero(); - std::array covariance = ekfCovarianceToTwistMessageCovariance(P); + Matrix6d p = Matrix6d::Zero(); + std::array covariance = ekf_covariance_to_twist_message_covariance(p); for (double e : covariance) { EXPECT_EQ(e, 0.); } diff --git a/localization/ekf_localizer/test/test_diagnostics.cpp b/localization/ekf_localizer/test/test_diagnostics.cpp index f506dca1cb230..ef0d7a6493756 100644 --- a/localization/ekf_localizer/test/test_diagnostics.cpp +++ b/localization/ekf_localizer/test/test_diagnostics.cpp @@ -16,20 +16,20 @@ #include -TEST(TestEkfDiagnostics, CheckProcessActivated) +TEST(TestEkfDiagnostics, check_process_activated) { diagnostic_msgs::msg::DiagnosticStatus stat; bool is_activated = true; - stat = checkProcessActivated(is_activated); + stat = check_process_activated(is_activated); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_activated = false; - stat = checkProcessActivated(is_activated); + stat = check_process_activated(is_activated); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestEkfDiagnostics, checkMeasurementUpdated) +TEST(TestEkfDiagnostics, check_measurement_updated) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -38,58 +38,58 @@ TEST(TestEkfDiagnostics, checkMeasurementUpdated) const size_t no_update_count_threshold_error = 250; size_t no_update_count = 0; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( 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( + stat = check_measurement_updated( 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( + stat = check_measurement_updated( 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( + stat = check_measurement_updated( 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( + stat = check_measurement_updated( 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( + stat = check_measurement_updated( 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) +TEST(TestEkfDiagnostics, check_measurement_queue_size) { 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); + stat = check_measurement_queue_size(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); + stat = check_measurement_queue_size(measurement_type, queue_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); } -TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) +TEST(TestEkfDiagnostics, check_measurement_delay_gate) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -98,17 +98,17 @@ TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) const double delay_time_threshold = 1.0; // not effect for stat.level bool is_passed_delay_gate = true; - stat = checkMeasurementDelayGate( + stat = check_measurement_delay_gate( 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( + stat = check_measurement_delay_gate( measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) +TEST(TestEkfDiagnostics, check_measurement_mahalanobis_gate) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -117,19 +117,19 @@ TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) const double mahalanobis_distance_threshold = 1.0; // not effect for stat.level bool is_passed_mahalanobis_gate = true; - stat = checkMeasurementMahalanobisGate( + stat = check_measurement_mahalanobis_gate( 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( + stat = check_measurement_mahalanobis_gate( measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, mahalanobis_distance_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) +TEST(TestLocalizationErrorMonitorDiagnostics, merge_diagnostic_status) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; std::vector stat_array(2); @@ -138,7 +138,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); EXPECT_EQ(merged_stat.message, "OK"); @@ -146,7 +146,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0"); @@ -154,7 +154,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN1"); @@ -162,7 +162,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); @@ -170,7 +170,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR1"); @@ -178,7 +178,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); @@ -186,7 +186,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(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_mahalanobis.cpp b/localization/ekf_localizer/test/test_mahalanobis.cpp index d208c1e8da06b..db7d538b533c3 100644 --- a/localization/ekf_localizer/test/test_mahalanobis.cpp +++ b/localization/ekf_localizer/test/test_mahalanobis.cpp @@ -18,44 +18,44 @@ constexpr double tolerance = 1e-8; -TEST(SquaredMahalanobis, SmokeTest) +TEST(squared_mahalanobis, SmokeTest) { { Eigen::Vector2d x(0, 1); Eigen::Vector2d y(3, 2); - Eigen::Matrix2d C; - C << 10, 0, 0, 10; + Eigen::Matrix2d c; + c << 10, 0, 0, 10; - EXPECT_NEAR(squaredMahalanobis(x, y, C), 1.0, tolerance); + EXPECT_NEAR(squared_mahalanobis(x, y, c), 1.0, tolerance); } { Eigen::Vector2d x(4, 1); Eigen::Vector2d y(1, 5); - Eigen::Matrix2d C; - C << 5, 0, 0, 5; + Eigen::Matrix2d c; + c << 5, 0, 0, 5; - EXPECT_NEAR(squaredMahalanobis(x, y, C), 5.0, tolerance); + EXPECT_NEAR(squared_mahalanobis(x, y, c), 5.0, tolerance); } } -TEST(Mahalanobis, SmokeTest) +TEST(mahalanobis, SmokeTest) { { Eigen::Vector2d x(0, 1); Eigen::Vector2d y(3, 2); - Eigen::Matrix2d C; - C << 10, 0, 0, 10; + Eigen::Matrix2d c; + c << 10, 0, 0, 10; - EXPECT_NEAR(mahalanobis(x, y, C), 1.0, tolerance); + EXPECT_NEAR(mahalanobis(x, y, c), 1.0, tolerance); } { Eigen::Vector2d x(4, 1); Eigen::Vector2d y(1, 5); - Eigen::Matrix2d C; - C << 5, 0, 0, 5; + Eigen::Matrix2d c; + c << 5, 0, 0, 5; - EXPECT_NEAR(mahalanobis(x, y, C), std::sqrt(5.0), tolerance); + EXPECT_NEAR(mahalanobis(x, y, c), std::sqrt(5.0), tolerance); } } diff --git a/localization/ekf_localizer/test/test_measurement.cpp b/localization/ekf_localizer/test/test_measurement.cpp index 9d63cb056d9d3..c77e39a67d15c 100644 --- a/localization/ekf_localizer/test/test_measurement.cpp +++ b/localization/ekf_localizer/test/test_measurement.cpp @@ -16,66 +16,66 @@ #include -TEST(Measurement, PoseMeasurementMatrix) +TEST(Measurement, pose_measurement_matrix) { - const Eigen::Matrix M = poseMeasurementMatrix(); + const Eigen::Matrix m = pose_measurement_matrix(); Eigen::Matrix expected; expected << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0; - EXPECT_EQ((M - expected).norm(), 0); + EXPECT_EQ((m - expected).norm(), 0); } -TEST(Measurement, TwistMeasurementMatrix) +TEST(Measurement, twist_measurement_matrix) { - const Eigen::Matrix M = twistMeasurementMatrix(); + const Eigen::Matrix m = twist_measurement_matrix(); Eigen::Matrix expected; expected << 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - EXPECT_EQ((M - expected).norm(), 0); + EXPECT_EQ((m - expected).norm(), 0); } -TEST(Measurement, PoseMeasurementCovariance) +TEST(Measurement, pose_measurement_covariance) { { const std::array covariance = {1, 2, 0, 0, 0, 3, 4, 5, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7, 8, 0, 0, 0, 9}; - const Eigen::Matrix3d M = poseMeasurementCovariance(covariance, 2); + const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2); Eigen::Matrix3d expected; expected << 2, 4, 6, 8, 10, 12, 14, 16, 18; - EXPECT_EQ((M - expected).norm(), 0.); + EXPECT_EQ((m - expected).norm(), 0.); } { // Make sure that other elements are not changed - std::array covariance; + std::array covariance{}; covariance.fill(0); - const Eigen::Matrix3d M = poseMeasurementCovariance(covariance, 2.); - EXPECT_EQ(M.norm(), 0); + const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2.); + EXPECT_EQ(m.norm(), 0); } } -TEST(Measurement, TwistMeasurementCovariance) +TEST(Measurement, twist_measurement_covariance) { { const std::array covariance = {1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 4}; - const Eigen::Matrix2d M = twistMeasurementCovariance(covariance, 2); + const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2); Eigen::Matrix2d expected; expected << 2, 4, 6, 8; - EXPECT_EQ((M - expected).norm(), 0.); + EXPECT_EQ((m - expected).norm(), 0.); } { // Make sure that other elements are not changed - std::array covariance; + std::array covariance{}; covariance.fill(0); - const Eigen::Matrix2d M = twistMeasurementCovariance(covariance, 2.); - EXPECT_EQ(M.norm(), 0); + const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2.); + EXPECT_EQ(m.norm(), 0); } } diff --git a/localization/ekf_localizer/test/test_numeric.cpp b/localization/ekf_localizer/test/test_numeric.cpp index baf4f46db79a9..f84c9aa1d0bf2 100644 --- a/localization/ekf_localizer/test/test_numeric.cpp +++ b/localization/ekf_localizer/test/test_numeric.cpp @@ -18,30 +18,30 @@ #include -TEST(Numeric, HasNan) +TEST(Numeric, has_nan) { const Eigen::VectorXd empty(0); const double inf = std::numeric_limits::infinity(); const double nan = std::nan(""); - EXPECT_FALSE(hasNan(empty)); - EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 0., 1.))); - EXPECT_FALSE(hasNan(Eigen::Vector3d(1e16, 0., 1.))); - EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 1., inf))); + EXPECT_FALSE(has_nan(empty)); + EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 0., 1.))); + EXPECT_FALSE(has_nan(Eigen::Vector3d(1e16, 0., 1.))); + EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 1., inf))); - EXPECT_TRUE(hasNan(Eigen::Vector3d(nan, 1., 0.))); + EXPECT_TRUE(has_nan(Eigen::Vector3d(nan, 1., 0.))); } -TEST(Numeric, HasInf) +TEST(Numeric, has_inf) { const Eigen::VectorXd empty(0); const double inf = std::numeric_limits::infinity(); const double nan = std::nan(""); - EXPECT_FALSE(hasInf(empty)); - EXPECT_FALSE(hasInf(Eigen::Vector3d(0., 0., 1.))); - EXPECT_FALSE(hasInf(Eigen::Vector3d(1e16, 0., 1.))); - EXPECT_FALSE(hasInf(Eigen::Vector3d(nan, 1., 0.))); + EXPECT_FALSE(has_inf(empty)); + EXPECT_FALSE(has_inf(Eigen::Vector3d(0., 0., 1.))); + EXPECT_FALSE(has_inf(Eigen::Vector3d(1e16, 0., 1.))); + EXPECT_FALSE(has_inf(Eigen::Vector3d(nan, 1., 0.))); - EXPECT_TRUE(hasInf(Eigen::Vector3d(0., 1., inf))); + EXPECT_TRUE(has_inf(Eigen::Vector3d(0., 1., inf))); } diff --git a/localization/ekf_localizer/test/test_state_transition.cpp b/localization/ekf_localizer/test/test_state_transition.cpp index 9cb7975a964a9..3b6bd93fd3dd8 100644 --- a/localization/ekf_localizer/test/test_state_transition.cpp +++ b/localization/ekf_localizer/test/test_state_transition.cpp @@ -17,43 +17,44 @@ #include "ekf_localizer/state_transition.hpp" #include -#include -TEST(StateTransition, NormalizeYaw) +#include + +TEST(StateTransition, normalize_yaw) { const double tolerance = 1e-6; - EXPECT_NEAR(normalizeYaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); - EXPECT_NEAR(normalizeYaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); - EXPECT_NEAR(normalizeYaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); - EXPECT_NEAR(normalizeYaw(M_PI * 4), M_PI * 0, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalize_yaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 4), M_PI * 0, tolerance); } -TEST(PredictNextState, PredictNextState) +TEST(predict_next_state, predict_next_state) { // This function is the definition of state transition so we just check // if the calculation is done according to the formula - Vector6d X_curr; - X_curr(0) = 2.; - X_curr(1) = 3.; - X_curr(2) = M_PI / 2.; - X_curr(3) = M_PI / 4.; - X_curr(4) = 10.; - X_curr(5) = 2. * M_PI / 3.; + Vector6d x_curr; + x_curr(0) = 2.; + x_curr(1) = 3.; + x_curr(2) = M_PI / 2.; + x_curr(3) = M_PI / 4.; + x_curr(4) = 10.; + x_curr(5) = 2. * M_PI / 3.; const double dt = 0.5; - const Vector6d X_next = predictNextState(X_curr, dt); + const Vector6d x_next = predict_next_state(x_curr, dt); const double tolerance = 1e-10; - EXPECT_NEAR(X_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); - EXPECT_NEAR(X_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); - EXPECT_NEAR(X_next(2), normalizeYaw(M_PI / 2. + M_PI / 3.), tolerance); - EXPECT_NEAR(X_next(3), X_curr(3), tolerance); - EXPECT_NEAR(X_next(4), X_curr(4), tolerance); - EXPECT_NEAR(X_next(5), X_curr(5), tolerance); + EXPECT_NEAR(x_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(x_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(x_next(2), normalize_yaw(M_PI / 2. + M_PI / 3.), tolerance); + EXPECT_NEAR(x_next(3), x_curr(3), tolerance); + EXPECT_NEAR(x_next(4), x_curr(4), tolerance); + EXPECT_NEAR(x_next(5), x_curr(5), tolerance); } -TEST(CreateStateTransitionMatrix, NumericalApproximation) +TEST(create_state_transition_matrix, NumericalApproximation) { // The transition matrix A = df / dx // We check if df = A * dx approximates f(x + dx) - f(x) @@ -64,10 +65,10 @@ TEST(CreateStateTransitionMatrix, NumericalApproximation) const Vector6d dx = 0.1 * Vector6d::Ones(); const Vector6d x = Vector6d::Zero(); - const Matrix6d A = createStateTransitionMatrix(x, dt); - const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + const Matrix6d a = create_state_transition_matrix(x, dt); + const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt); - EXPECT_LT((df - A * dx).norm(), 2e-3); + EXPECT_LT((df - a * dx).norm(), 2e-3); } { @@ -76,20 +77,20 @@ TEST(CreateStateTransitionMatrix, NumericalApproximation) const Vector6d dx = 0.1 * Vector6d::Ones(); const Vector6d x = (Vector6d() << 0.1, 0.2, 0.1, 0.4, 0.1, 0.3).finished(); - const Matrix6d A = createStateTransitionMatrix(x, dt); - const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + const Matrix6d a = create_state_transition_matrix(x, dt); + const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt); - EXPECT_LT((df - A * dx).norm(), 5e-3); + EXPECT_LT((df - a * dx).norm(), 5e-3); } } -TEST(ProcessNoiseCovariance, ProcessNoiseCovariance) +TEST(process_noise_covariance, process_noise_covariance) { - const Matrix6d Q = processNoiseCovariance(1., 2., 3.); - EXPECT_EQ(Q(2, 2), 1.); // for yaw - EXPECT_EQ(Q(4, 4), 2.); // for vx - EXPECT_EQ(Q(5, 5), 3.); // for wz + const Matrix6d q = process_noise_covariance(1., 2., 3.); + EXPECT_EQ(q(2, 2), 1.); // for yaw + EXPECT_EQ(q(4, 4), 2.); // for vx + EXPECT_EQ(q(5, 5), 3.); // for wz // Make sure other elements are zero - EXPECT_EQ(processNoiseCovariance(0, 0, 0).norm(), 0.); + EXPECT_EQ(process_noise_covariance(0, 0, 0).norm(), 0.); } diff --git a/localization/ekf_localizer/test/test_string.cpp b/localization/ekf_localizer/test/test_string.cpp index 7b35a56d889e9..013072e5145c7 100644 --- a/localization/ekf_localizer/test/test_string.cpp +++ b/localization/ekf_localizer/test/test_string.cpp @@ -16,11 +16,11 @@ #include -TEST(EraseLeadingSlash, SmokeTest) +TEST(erase_leading_slash, SmokeTest) { - EXPECT_EQ(eraseLeadingSlash("/topic"), "topic"); - EXPECT_EQ(eraseLeadingSlash("topic"), "topic"); // do nothing + EXPECT_EQ(erase_leading_slash("/topic"), "topic"); + EXPECT_EQ(erase_leading_slash("topic"), "topic"); // do nothing - EXPECT_EQ(eraseLeadingSlash(""), ""); - EXPECT_EQ(eraseLeadingSlash("/"), ""); + EXPECT_EQ(erase_leading_slash(""), ""); + EXPECT_EQ(erase_leading_slash("/"), ""); } diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index 2069969e0ae5e..ec7d3420d7f79 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -18,45 +18,45 @@ #include -TEST(PoseDelayStepWarningMessage, SmokeTest) +TEST(pose_delay_step_warning_message, SmokeTest) { EXPECT_STREQ( - poseDelayStepWarningMessage(6.0, 4.0).c_str(), + pose_delay_step_warning_message(6.0, 4.0).c_str(), "Pose delay exceeds the compensation limit, ignored. " "delay: 6.000[s], limit: 4.000[s]"); } -TEST(TwistDelayStepWarningMessage, SmokeTest) +TEST(twist_delay_step_warning_message, SmokeTest) { EXPECT_STREQ( - twistDelayStepWarningMessage(10.0, 6.0).c_str(), + twist_delay_step_warning_message(10.0, 6.0).c_str(), "Twist delay exceeds the compensation limit, ignored. " "delay: 10.000[s], limit: 6.000[s]"); } -TEST(PoseDelayTimeWarningMessage, SmokeTest) +TEST(pose_delay_time_warning_message, SmokeTest) { EXPECT_STREQ( - poseDelayTimeWarningMessage(-1.0).c_str(), + pose_delay_time_warning_message(-1.0).c_str(), "Pose time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); EXPECT_STREQ( - poseDelayTimeWarningMessage(-0.4).c_str(), + pose_delay_time_warning_message(-0.4).c_str(), "Pose time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); } -TEST(TwistDelayTimeWarningMessage, SmokeTest) +TEST(twist_delay_time_warning_message, SmokeTest) { EXPECT_STREQ( - twistDelayTimeWarningMessage(-1.0).c_str(), + twist_delay_time_warning_message(-1.0).c_str(), "Twist time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); EXPECT_STREQ( - twistDelayTimeWarningMessage(-0.4).c_str(), + twist_delay_time_warning_message(-0.4).c_str(), "Twist time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); } -TEST(MahalanobisWarningMessage, SmokeTest) +TEST(mahalanobis_warning_message, SmokeTest) { EXPECT_STREQ( - mahalanobisWarningMessage(1.0, 0.5).c_str(), + mahalanobis_warning_message(1.0, 0.5).c_str(), "The Mahalanobis distance 1.0000 is over the limit 0.5000."); } diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index a832383ff4761..1b142b254d2e1 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp + src/diagnostics_module.cpp ) target_link_libraries(${PROJECT_NAME} fmt) diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index e9e390010f084..aa6f2a96f4838 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -34,3 +34,18 @@ - [Limitation] The frequency of the output messages depends on the frequency of the input IMU message. - [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix. + +## Diagnostics + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| -------------------------------- | ----------------------------------------------------------------------------------------- | ------------------------------- | ------------------------------------------------- | +| `topic_time_stamp` | the time stamp of service calling. [nano second] | none | none | +| `is_arrived_first_vehicle_twist` | whether the vehicle twist topic has been received even once. | not arrive yet | none | +| `is_arrived_first_imu` | whether the imu topic has been received even once. | not arrive yet | none | +| `vehicle_twist_time_stamp_dt` | the time difference between the current time and the latest vehicle twist topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `imu_time_stamp_dt` | the time difference between the current time and the latest imu topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `vehicle_twist_queue_size` | the size of vehicle_twist_queue. | none | none | +| `imu_queue_size` | the size of gyro_queue. | none | none | +| `is_succeed_transform_imu` | whether transform imu is succeed or not. | none | failed | diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp new file mode 100644 index 0000000000000..c5b7b7a592013 --- /dev/null +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -0,0 +1,65 @@ +// 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 GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ +#define GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +class DiagnosticsModule +{ +public: + DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + void clear(); + void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); + template + void add_key_value(const std::string & key, const T & value); + void update_level_and_message(const int8_t level, const std::string & message); + void publish(const rclcpp::Time & publish_time_stamp); + +private: + [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array( + const rclcpp::Time & publish_time_stamp) const; + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_; +}; + +template +void DiagnosticsModule::add_key_value(const std::string & key, const T & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = std::to_string(value); + add_key_value(key_value); +} + +template <> +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value); +template <> +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); + +} // namespace autoware::gyro_odometer + +#endif // GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ 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 2926589bd2da9..f85640ab2c523 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,6 +15,7 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "gyro_odometer/diagnostics_module.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" @@ -46,13 +47,13 @@ class GyroOdometerNode : public rclcpp::Node public: explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); - ~GyroOdometerNode(); private: - void callbackVehicleTwist( + void callback_vehicle_twist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); - void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); - void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); + void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + void concat_gyro_and_odometer(); + void publish_data(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); rclcpp::Subscription::SharedPtr vehicle_twist_sub_; @@ -74,8 +75,12 @@ class GyroOdometerNode : public rclcpp::Node bool vehicle_twist_arrived_; bool imu_arrived_; + rclcpp::Time latest_vehicle_twist_ros_time_; + rclcpp::Time latest_imu_ros_time_; std::deque vehicle_twist_queue_; std::deque gyro_queue_; + + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/media/diagnostic.png b/localization/gyro_odometer/media/diagnostic.png new file mode 100644 index 0000000000000..f2324f4079d0d Binary files /dev/null and b/localization/gyro_odometer/media/diagnostic.png differ diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index 3c979eb69ac8a..575f6a2d74837 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + diagnostic_msgs fmt geometry_msgs rclcpp diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp new file mode 100644 index 0000000000000..5d687943cef53 --- /dev/null +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -0,0 +1,110 @@ +// 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 "gyro_odometer/diagnostics_module.hpp" + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +: clock_(node->get_clock()) +{ + diagnostics_pub_ = + node->create_publisher("/diagnostics", 10); + + diagnostics_status_msg_.name = + std::string(node->get_name()) + std::string(": ") + diagnostic_name; + diagnostics_status_msg_.hardware_id = node->get_name(); +} + +void DiagnosticsModule::clear() +{ + diagnostics_status_msg_.values.clear(); + diagnostics_status_msg_.values.shrink_to_fit(); + + diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_status_msg_.message = ""; +} + +void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) +{ + auto it = std::find_if( + std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), + [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; }); + + if (it != std::cend(diagnostics_status_msg_.values)) { + it->value = key_value_msg.value; + } else { + diagnostics_status_msg_.values.push_back(key_value_msg); + } +} + +template <> +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value; + add_key_value(key_value); +} + +template <> +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value ? "True" : "False"; + add_key_value(key_value); +} + +void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) +{ + if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!diagnostics_status_msg_.message.empty()) { + diagnostics_status_msg_.message += "; "; + } + diagnostics_status_msg_.message += message; + } + if (level > diagnostics_status_msg_.level) { + diagnostics_status_msg_.level = level; + } +} + +void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) +{ + diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); +} + +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( + const rclcpp::Time & publish_time_stamp) const +{ + diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; + diagnostics_msg.header.stamp = publish_time_stamp; + diagnostics_msg.status.push_back(diagnostics_status_msg_); + + if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + diagnostics_msg.status.at(0).message = "OK"; + } + + return diagnostics_msg; +} + +} // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index bda7af74b8489..0ec479770740f 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -25,11 +25,26 @@ #include #include +#include #include namespace autoware::gyro_odometer { +std::array transform_covariance(const std::array & cov) +{ + using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + + double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); + + std::array cov_transformed = {}; + cov_transformed.fill(0.); + cov_transformed[COV_IDX::X_X] = max_cov; + cov_transformed[COV_IDX::Y_Y] = max_cov; + cov_transformed[COV_IDX::Z_Z] = max_cov; + return cov_transformed; +} + GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) : Node("gyro_odometer", node_options), output_frame_(declare_parameter("output_frame")), @@ -42,11 +57,11 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_vehicle_twist, this, std::placeholders::_1)); imu_sub_ = create_subscription( "imu", rclcpp::QoS{100}, - std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_imu, this, std::placeholders::_1)); twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); twist_with_covariance_raw_pub_ = create_publisher( @@ -56,46 +71,159 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); + diagnostics_ = std::make_unique(this, "gyro_odometer_status"); + // TODO(YamatoAndo) createTimer } -GyroOdometerNode::~GyroOdometerNode() +void GyroOdometerNode::callback_vehicle_twist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr) { + diagnostics_->clear(); + diagnostics_->add_key_value( + "topic_time_stamp", + static_cast(vehicle_twist_msg_ptr->header.stamp).nanoseconds()); + + vehicle_twist_arrived_ = true; + latest_vehicle_twist_ros_time_ = vehicle_twist_msg_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_msg_ptr); + concat_gyro_and_odometer(); + + diagnostics_->publish(vehicle_twist_msg_ptr->header.stamp); } -std::array transformCovariance(const std::array & cov) +void GyroOdometerNode::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + diagnostics_->clear(); + diagnostics_->add_key_value( + "topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); - double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); + imu_arrived_ = true; + latest_imu_ros_time_ = imu_msg_ptr->header.stamp; + gyro_queue_.push_back(*imu_msg_ptr); + concat_gyro_and_odometer(); - std::array cov_transformed; - cov_transformed.fill(0.); - cov_transformed[COV_IDX::X_X] = max_cov; - cov_transformed[COV_IDX::Y_Y] = max_cov; - cov_transformed[COV_IDX::Z_Z] = max_cov; - return cov_transformed; + diagnostics_->publish(imu_msg_ptr->header.stamp); } -geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( - const std::deque & vehicle_twist_queue, - const std::deque & gyro_queue) +void GyroOdometerNode::concat_gyro_and_odometer() { + // check arrive first topic + diagnostics_->add_key_value("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->add_key_value("is_arrived_first_imu", imu_arrived_); + if (!vehicle_twist_arrived_) { + std::stringstream message; + message << "Twist msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); + return; + } + if (!imu_arrived_) { + std::stringstream message; + message << "Imu msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); + return; + } + + // check timeout + const double vehicle_twist_dt = + std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds()); + const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds()); + diagnostics_->add_key_value("vehicle_twist_time_stamp_dt", vehicle_twist_dt); + diagnostics_->add_key_value("imu_time_stamp_dt", imu_dt); + if (vehicle_twist_dt > message_timeout_sec_) { + const std::string message = fmt::format( + "Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]", + vehicle_twist_dt, message_timeout_sec_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); + + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); + return; + } + if (imu_dt > message_timeout_sec_) { + const std::string message = fmt::format( + "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); + + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); + return; + } + + // check queue size + diagnostics_->add_key_value("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->add_key_value("imu_queue_size", gyro_queue_.size()); + if (vehicle_twist_queue_.empty()) { + // not output error and clear queue + return; + } + if (gyro_queue_.empty()) { + // not output error and clear queue + return; + } + + // get transformation + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = + transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_); + + const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr); + diagnostics_->add_key_value("is_succeed_transform_imu", is_succeed_transform_imu); + if (!is_succeed_transform_imu) { + std::stringstream message; + message << "Please publish TF " << output_frame_ << " to " + << gyro_queue_.front().header.frame_id; + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + + vehicle_twist_queue_.clear(); + gyro_queue_.clear(); + return; + } + + // transform gyro frame + for (auto & gyro : gyro_queue_) { + geometry_msgs::msg::Vector3Stamped angular_velocity; + angular_velocity.header = gyro.header; + angular_velocity.vector = gyro.angular_velocity; + + geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; + transformed_angular_velocity.header = tf_imu2base_ptr->header; + tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); + + gyro.header.frame_id = output_frame_; + gyro.angular_velocity = transformed_angular_velocity.vector; + gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance); + } + using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + // calc mean, covariance double vx_mean = 0; geometry_msgs::msg::Vector3 gyro_mean{}; double vx_covariance_original = 0; geometry_msgs::msg::Vector3 gyro_covariance_original{}; - for (const auto & vehicle_twist : vehicle_twist_queue) { + for (const auto & vehicle_twist : vehicle_twist_queue_) { vx_mean += vehicle_twist.twist.twist.linear.x; vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; } - vx_mean /= vehicle_twist_queue.size(); - vx_covariance_original /= vehicle_twist_queue.size(); + vx_mean /= static_cast(vehicle_twist_queue_.size()); + vx_covariance_original /= static_cast(vehicle_twist_queue_.size()); - for (const auto & gyro : gyro_queue) { + for (const auto & gyro : gyro_queue_) { gyro_mean.x += gyro.angular_velocity.x; gyro_mean.y += gyro.angular_velocity.y; gyro_mean.z += gyro.angular_velocity.z; @@ -103,151 +231,46 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; } - gyro_mean.x /= gyro_queue.size(); - gyro_mean.y /= gyro_queue.size(); - gyro_mean.z /= gyro_queue.size(); - gyro_covariance_original.x /= gyro_queue.size(); - gyro_covariance_original.y /= gyro_queue.size(); - gyro_covariance_original.z /= gyro_queue.size(); - + gyro_mean.x /= static_cast(gyro_queue_.size()); + gyro_mean.y /= static_cast(gyro_queue_.size()); + gyro_mean.z /= static_cast(gyro_queue_.size()); + gyro_covariance_original.x /= static_cast(gyro_queue_.size()); + gyro_covariance_original.y /= static_cast(gyro_queue_.size()); + gyro_covariance_original.z /= static_cast(gyro_queue_.size()); + + // concat geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; - const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue.back().header.stamp); - const auto latest_imu_stamp = rclcpp::Time(gyro_queue.back().header.stamp); + const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue_.back().header.stamp); + const auto latest_imu_stamp = rclcpp::Time(gyro_queue_.back().header.stamp); if (latest_vehicle_twist_stamp < latest_imu_stamp) { twist_with_cov.header.stamp = latest_imu_stamp; } else { twist_with_cov.header.stamp = latest_vehicle_twist_stamp; } - twist_with_cov.header.frame_id = gyro_queue.front().header.frame_id; + twist_with_cov.header.frame_id = gyro_queue_.front().header.frame_id; twist_with_cov.twist.twist.linear.x = vx_mean; twist_with_cov.twist.twist.angular = gyro_mean; // From a statistical point of view, here we reduce the covariances according to the number of // observed data twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = - vx_covariance_original / vehicle_twist_queue.size(); + vx_covariance_original / static_cast(vehicle_twist_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = - gyro_covariance_original.x / gyro_queue.size(); + gyro_covariance_original.x / static_cast(gyro_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue.size(); + gyro_covariance_original.y / static_cast(gyro_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue.size(); - - return twist_with_cov; -} - -void GyroOdometerNode::callbackVehicleTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) -{ - vehicle_twist_arrived_ = true; - if (!imu_arrived_) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed"); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; - } - - const double twist_dt = std::abs((this->now() - vehicle_twist_ptr->header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; - } - - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); - - if (gyro_queue_.empty()) return; - const double imu_dt = std::abs((this->now() - gyro_queue_.back().header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; - } + gyro_covariance_original.z / static_cast(gyro_queue_.size()); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); -} - -void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) -{ - imu_arrived_ = true; - if (!vehicle_twist_arrived_) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed"); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; - } - - const double imu_dt = std::abs((this->now() - imu_msg_ptr->header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; - } - - geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = - transform_listener_->getLatestTransform(imu_msg_ptr->header.frame_id, output_frame_); - if (!tf_imu2base_ptr) { - RCLCPP_ERROR( - this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), - (imu_msg_ptr->header.frame_id).c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; - } - - geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.header = imu_msg_ptr->header; - angular_velocity.vector = imu_msg_ptr->angular_velocity; - - geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - transformed_angular_velocity.header = tf_imu2base_ptr->header; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); - - sensor_msgs::msg::Imu gyro_base_link; - gyro_base_link.header = imu_msg_ptr->header; - gyro_base_link.header.frame_id = output_frame_; - gyro_base_link.angular_velocity = transformed_angular_velocity.vector; - gyro_base_link.angular_velocity_covariance = - transformCovariance(imu_msg_ptr->angular_velocity_covariance); - - gyro_queue_.push_back(gyro_base_link); - - if (vehicle_twist_queue_.empty()) return; - const double twist_dt = - std::abs((this->now() - vehicle_twist_queue_.back().header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; - } + publish_data(twist_with_cov); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); vehicle_twist_queue_.clear(); gyro_queue_.clear(); } -void GyroOdometerNode::publishData( +void GyroOdometerNode::publish_data( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp index 54e1d320319d3..f55278998f5f0 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp @@ -17,7 +17,7 @@ using geometry_msgs::msg::TwistWithCovarianceStamped; using sensor_msgs::msg::Imu; -Imu generateSampleImu() +Imu generate_sample_imu() { Imu imu; imu.header.frame_id = "base_link"; @@ -27,7 +27,7 @@ Imu generateSampleImu() return imu; } -TwistWithCovarianceStamped generateSampleVelocity() +TwistWithCovarianceStamped generate_sample_velocity() { TwistWithCovarianceStamped twist; twist.header.frame_id = "base_link"; @@ -35,7 +35,7 @@ TwistWithCovarianceStamped generateSampleVelocity() return twist; } -rclcpp::NodeOptions getNodeOptionsWithDefaultParams() +rclcpp::NodeOptions get_node_options_with_default_params() { rclcpp::NodeOptions node_options; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp index 6e3aff0b841a9..9da344e410e61 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp @@ -20,11 +20,8 @@ #include #include -using geometry_msgs::msg::TwistWithCovarianceStamped; -using sensor_msgs::msg::Imu; - -Imu generateSampleImu(); -TwistWithCovarianceStamped generateSampleVelocity(); -rclcpp::NodeOptions getNodeOptionsWithDefaultParams(); +sensor_msgs::msg::Imu generate_sample_imu(); +geometry_msgs::msg::TwistWithCovarianceStamped generate_sample_velocity(); +rclcpp::NodeOptions get_node_options_with_default_params(); #endif // TEST_GYRO_ODOMETER_HELPER_HPP_ diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index b7849ef03bfc5..fc331a638a1dd 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -50,19 +50,22 @@ class VelocityGenerator : public rclcpp::Node class GyroOdometerValidator : public rclcpp::Node { public: - GyroOdometerValidator() : Node("gyro_odometer_validator"), received_latest_twist_ptr(nullptr) - { - twist_sub = create_subscription( - "/twist_with_covariance", 1, [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { + GyroOdometerValidator() + : Node("gyro_odometer_validator"), + twist_sub(create_subscription( + "/twist_with_covariance", 1, + [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { received_latest_twist_ptr = msg; - }); + })), + received_latest_twist_ptr(nullptr) + { } rclcpp::Subscription::SharedPtr twist_sub; TwistWithCovarianceStamped::ConstSharedPtr received_latest_twist_ptr; }; -void spinSome(rclcpp::Node::SharedPtr node_ptr) +void wait_spin_some(rclcpp::Node::SharedPtr node_ptr) { for (int i = 0; i < 50; ++i) { rclcpp::spin_some(node_ptr); @@ -70,7 +73,7 @@ void spinSome(rclcpp::Node::SharedPtr node_ptr) } } -bool isTwistValid( +bool is_twist_valid( const TwistWithCovarianceStamped & twist, const TwistWithCovarianceStamped & twist_ground_truth) { if (twist.twist.twist.linear.x != twist_ground_truth.twist.twist.linear.x) { @@ -99,8 +102,8 @@ bool isTwistValid( // velocity data are provided TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) { - Imu input_imu = generateSampleImu(); - TwistWithCovarianceStamped input_velocity = generateSampleVelocity(); + Imu input_imu = generate_sample_imu(); + TwistWithCovarianceStamped input_velocity = generate_sample_velocity(); TwistWithCovarianceStamped expected_output_twist; expected_output_twist.twist.twist.linear.x = input_velocity.twist.twist.linear.x; @@ -108,8 +111,8 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y; expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; - auto gyro_odometer_node = - std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = std::make_shared( + get_node_options_with_default_params()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -120,13 +123,13 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) velocity_generator->vehicle_velocity_pub->publish(input_velocity); // gyro_odometer receives IMU and velocity, and publishes the fused twist data. - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node receives the fused twist data and store in "received_latest_twist_ptr". - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_FALSE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); - EXPECT_TRUE(isTwistValid( + EXPECT_TRUE(is_twist_valid( *(gyro_odometer_validator_node->received_latest_twist_ptr), expected_output_twist)); } @@ -134,20 +137,20 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) // Verify that the gyro_odometer does NOT publish any outputs when only IMU is provided TEST(GyroOdometer, TestGyroOdometerImuOnly) { - Imu input_imu = generateSampleImu(); + Imu input_imu = generate_sample_imu(); - auto gyro_odometer_node = - std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = std::make_shared( + get_node_options_with_default_params()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); imu_generator->imu_pub->publish(input_imu); // gyro_odometer receives IMU - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node waits for the output fused twist from gyro_odometer - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_TRUE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); } diff --git a/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp index 4fe4e5a5aac14..b1da87128bee5 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/diagnostics.hpp @@ -20,12 +20,12 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy_lateral_direction( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size); -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array); #endif // LOCALIZATION_ERROR_MONITOR__DIAGNOSTICS_HPP_ diff --git a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp index 6016677d88136..53f243520742f 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -52,13 +52,12 @@ class LocalizationErrorMonitor : public rclcpp::Node double warn_ellipse_size_lateral_direction_; Ellipse ellipse_; - void onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg); - visualization_msgs::msg::Marker createEllipseMarker( + void on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg); + visualization_msgs::msg::Marker create_ellipse_marker( const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom); - double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); + static double measure_size_ellipse_along_body_frame(const Eigen::Matrix2d & Pinv, double theta); public: explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options); - ~LocalizationErrorMonitor() = default; }; #endif // LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ diff --git a/localization/localization_error_monitor/src/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp index 375fccfa06787..e6b9da8fc4a97 100644 --- a/localization/localization_error_monitor/src/diagnostics.cpp +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -17,7 +17,7 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -41,7 +41,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection( +diagnostic_msgs::msg::DiagnosticStatus check_localization_accuracy_lateral_direction( const double ellipse_size, const double warn_ellipse_size, const double error_ellipse_size) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -66,7 +66,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection } // The highest level within the stat_array will be reflected in the merged_stat. -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; diff --git a/localization/localization_error_monitor/src/localization_error_monitor.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp index 7a1cd0b213c39..04bb37a85e41b 100644 --- a/localization/localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -45,7 +45,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o this->declare_parameter("warn_ellipse_size_lateral_direction"); odom_sub_ = this->create_subscription( - "input/odom", 1, std::bind(&LocalizationErrorMonitor::onOdom, this, std::placeholders::_1)); + "input/odom", 1, std::bind(&LocalizationErrorMonitor::on_odom, this, std::placeholders::_1)); // QoS setup rclcpp::QoS durable_qos(1); @@ -58,7 +58,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); } -visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( +visualization_msgs::msg::Marker LocalizationErrorMonitor::create_ellipse_marker( const Ellipse & ellipse, nav_msgs::msg::Odometry::ConstSharedPtr odom) { tf2::Quaternion quat; @@ -85,7 +85,7 @@ visualization_msgs::msg::Marker LocalizationErrorMonitor::createEllipseMarker( return marker; } -void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) +void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) { // create xy covariance (2x2 matrix) // input geometry_msgs::PoseWithCovariance contain 6x6 matrix @@ -110,21 +110,21 @@ void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr in ellipse_.P = xy_covariance; const double yaw_vehicle = tf2::getYaw(input_msg->pose.pose.orientation); ellipse_.size_lateral_direction = - scale_ * measureSizeEllipseAlongBodyFrame(ellipse_.P.inverse(), yaw_vehicle); + scale_ * measure_size_ellipse_along_body_frame(ellipse_.P.inverse(), yaw_vehicle); - const auto ellipse_marker = createEllipseMarker(ellipse_, input_msg); + const auto ellipse_marker = create_ellipse_marker(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( + check_localization_accuracy(ellipse_.long_radius, warn_ellipse_size_, error_ellipse_size_)); + diag_status_array.push_back(check_localization_accuracy_lateral_direction( 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 = merge_diagnostic_status(diag_status_array); diag_merged_status.name = "localization: " + std::string(this->get_name()); diag_merged_status.hardware_id = this->get_name(); @@ -134,7 +134,7 @@ void LocalizationErrorMonitor::onOdom(nav_msgs::msg::Odometry::ConstSharedPtr in diag_pub_->publish(diag_msg); } -double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( +double LocalizationErrorMonitor::measure_size_ellipse_along_body_frame( const Eigen::Matrix2d & Pinv, const double theta) { Eigen::MatrixXd e(2, 1); diff --git a/localization/localization_error_monitor/test/test_diagnostics.cpp b/localization/localization_error_monitor/test/test_diagnostics.cpp index 7b7e8aaab4ed9..12515687e7a98 100644 --- a/localization/localization_error_monitor/test/test_diagnostics.cpp +++ b/localization/localization_error_monitor/test/test_diagnostics.cpp @@ -24,23 +24,23 @@ TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracy) const double error_ellipse_size = 1.0; double ellipse_size = 0.0; - stat = checkLocalizationAccuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy(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); + stat = check_localization_accuracy(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); + stat = check_localization_accuracy(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); + stat = check_localization_accuracy(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); + stat = check_localization_accuracy(ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); } @@ -52,28 +52,28 @@ TEST(TestLocalizationErrorMonitorDiagnostics, CheckLocalizationAccuracyLateralDi const double error_ellipse_size = 0.3; double ellipse_size = 0.0; - stat = - checkLocalizationAccuracyLateralDirection(ellipse_size, warn_ellipse_size, error_ellipse_size); + stat = check_localization_accuracy_lateral_direction( + 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); + stat = check_localization_accuracy_lateral_direction( + 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); + stat = check_localization_accuracy_lateral_direction( + 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); + stat = check_localization_accuracy_lateral_direction( + 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); + stat = check_localization_accuracy_lateral_direction( + ellipse_size, warn_ellipse_size, error_ellipse_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); } @@ -86,7 +86,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); EXPECT_EQ(merged_stat.message, "OK"); @@ -94,7 +94,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0"); @@ -102,7 +102,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN1"); @@ -110,7 +110,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); @@ -118,7 +118,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR1"); @@ -126,7 +126,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); @@ -134,7 +134,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) 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); + merged_stat = merge_diagnostic_status(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/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index ae6f0b61f063c..133442df68dcd 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -28,11 +28,11 @@ std_msgs::msg::ColorRGBA exchange_color_crc(double x) color.b = 1.0; color.g = static_cast(std::sin(x * 2.0 * M_PI)); color.r = 0; - } else if (x > 0.25 && x <= 0.5) { + } else if (x <= 0.5) { color.b = static_cast(std::sin(x * 2 * M_PI)); color.g = 1.0; color.r = 0; - } else if (x > 0.5 && x <= 0.75) { + } else if (x <= 0.75) { color.b = 0; color.g = 1.0; color.r = static_cast(-std::sin(x * 2.0 * M_PI)); diff --git a/localization/localization_util/test/test_smart_pose_buffer.cpp b/localization/localization_util/test/test_smart_pose_buffer.cpp index 2520e458f2d33..a8ed6d98b6064 100644 --- a/localization/localization_util/test/test_smart_pose_buffer.cpp +++ b/localization/localization_util/test/test_smart_pose_buffer.cpp @@ -42,15 +42,7 @@ bool compare_pose( pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; } -class TestSmartPoseBuffer : public ::testing::Test -{ -protected: - void SetUp() override {} - - void TearDown() override {} -}; - -TEST_F(TestSmartPoseBuffer, interpolate_pose) // NOLINT +TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT { rclcpp::Logger logger = rclcpp::get_logger("test_logger"); const double pose_timeout_sec = 10.0; diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index a44db7bbaa4bf..8049104e8f2f5 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -262,23 +262,27 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num drawing -| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | -| ----------------------------------------- | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- | -| `topic_time_stamp` | the time stamp of input topic | none | none | no | -| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | -| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | -| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | -| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | -| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. | -| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | -| `is_set_map_points` | whether the map points is set or not | not set | none | yes | -| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | -| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | -| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | -| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | -| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | -| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | -| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - | +| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | +| ------------------------------------------------ | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- | +| `topic_time_stamp` | the time stamp of input topic | none | none | no | +| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | +| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | +| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | +| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | +| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. | +| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | +| `is_set_map_points` | whether the map points is set or not | not set | none | yes | +| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | +| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | +| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | +| `transform_probability_diff` | the tp score difference for the current ndt optimization | none | none | no | +| `transform_probability_before` | the tp score before the current ndt optimization | none | none | no | +| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | +| `nearest_voxel_transformation_likelihood_diff` | the nvtl score difference for the current ndt optimization | none | none | no | +| `nearest_voxel_transformation_likelihood_before` | the nvtl score before the current ndt optimization | none | none | no | +| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | +| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | +| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - | ※The `sensor_points_callback` shares the same callback group as the `trigger_node_service` and `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale. diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp index 4f7b5eff6521b..6dfea386abaf8 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/diagnostics_module.hpp @@ -27,14 +27,14 @@ class DiagnosticsModule public: DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); - void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); template - void addKeyValue(const std::string & key, const T & value); - void updateLevelAndMessage(const int8_t level, const std::string & message); + void add_key_value(const std::string & key, const T & value); + void update_level_and_message(const int8_t level, const std::string & message); void publish(const rclcpp::Time & publish_time_stamp); private: - diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const; rclcpp::Clock::SharedPtr clock_; @@ -44,17 +44,17 @@ class DiagnosticsModule }; template -void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +void DiagnosticsModule::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = std::to_string(value); - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value); template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); #endif // NDT_SCAN_MATCHER__DIAGNOSTICS_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp index bc7bf1fe40d36..7002b4bf43a73 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/hyper_parameters.hpp @@ -33,62 +33,62 @@ struct HyperParameters { struct Frame { - std::string base_frame; - std::string ndt_base_frame; - std::string map_frame; - } frame; + std::string base_frame{}; + std::string ndt_base_frame{}; + std::string map_frame{}; + } frame{}; struct SensorPoints { - double required_distance; - } sensor_points; + double required_distance{}; + } sensor_points{}; - pclomp::NdtParams ndt; - bool ndt_regularization_enable; + pclomp::NdtParams ndt{}; + bool ndt_regularization_enable{}; struct InitialPoseEstimation { - int64_t particles_num; - int64_t n_startup_trials; - } initial_pose_estimation; + int64_t particles_num{}; + int64_t n_startup_trials{}; + } initial_pose_estimation{}; struct Validation { - double lidar_topic_timeout_sec; - double initial_pose_timeout_sec; - double initial_pose_distance_tolerance_m; - double critical_upper_bound_exe_time_ms; - } validation; + double lidar_topic_timeout_sec{}; + double initial_pose_timeout_sec{}; + double initial_pose_distance_tolerance_m{}; + double critical_upper_bound_exe_time_ms{}; + } validation{}; struct ScoreEstimation { - ConvergedParamType converged_param_type; - double converged_param_transform_probability; - double converged_param_nearest_voxel_transformation_likelihood; + ConvergedParamType converged_param_type{}; + double converged_param_transform_probability{}; + double converged_param_nearest_voxel_transformation_likelihood{}; struct NoGroundPoints { - bool enable; - double z_margin_for_ground_removal; - } no_ground_points; - } score_estimation; + bool enable{}; + double z_margin_for_ground_removal{}; + } no_ground_points{}; + } score_estimation{}; struct Covariance { - std::array output_pose_covariance; + std::array output_pose_covariance{}; struct CovarianceEstimation { - bool enable; - std::vector initial_pose_offset_model; - } covariance_estimation; - } covariance; + bool enable{}; + std::vector initial_pose_offset_model{}; + } covariance_estimation{}; + } covariance{}; struct DynamicMapLoading { - double update_distance; - double map_radius; - double lidar_radius; - } dynamic_map_loading; + double update_distance{}; + double map_radius{}; + double lidar_radius{}; + } dynamic_map_loading{}; public: explicit HyperParameters(rclcpp::Node * node) 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 4f8042d106c75..cbd2797c57922 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 @@ -131,8 +131,6 @@ class NDTScanMatcher : public rclcpp::Node static int count_oscillation(const std::vector & result_pose_msg_array); - std::array rotate_covariance( - const std::array & src_covariance, const Eigen::Matrix3d & rotation) const; std::array estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); diff --git a/localization/ndt_scan_matcher/src/diagnostics_module.cpp b/localization/ndt_scan_matcher/src/diagnostics_module.cpp index 1e08ebb89f51e..805ee676c5e04 100644 --- a/localization/ndt_scan_matcher/src/diagnostics_module.cpp +++ b/localization/ndt_scan_matcher/src/diagnostics_module.cpp @@ -41,7 +41,7 @@ void DiagnosticsModule::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) { auto it = std::find_if( std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), @@ -55,24 +55,24 @@ void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_v } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value; - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value ? "True" : "False"; - addKeyValue(key_value); + add_key_value(key_value); } -void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -87,10 +87,10 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) { - diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); + diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 40f0b1f4465fa..32e5a0f2a7c3c 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -53,23 +53,23 @@ void MapUpdateModule::callback_timer( std::unique_ptr & diagnostics_ptr) { // check is_activated - diagnostics_ptr->addKeyValue("is_activated", is_activated); + diagnostics_ptr->add_key_value("is_activated", is_activated); if (!is_activated) { std::stringstream message; message << "Node is not activated."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } // check is_set_last_update_position const bool is_set_last_update_position = (position != std::nullopt); - diagnostics_ptr->addKeyValue("is_set_last_update_position", is_set_last_update_position); + diagnostics_ptr->add_key_value("is_set_last_update_position", is_set_last_update_position); if (!is_set_last_update_position) { std::stringstream message; message << "Cannot find the reference position for map update." << "Please check if the EKF odometry is provided to NDT."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -92,11 +92,11 @@ bool MapUpdateModule::should_update_map( const double distance = std::hypot(dx, dy); // check distance_last_update_position_to_current_position - diagnostics_ptr->addKeyValue("distance_last_update_position_to_current_position", distance); + diagnostics_ptr->add_key_value("distance_last_update_position_to_current_position", distance); if (distance + param_.lidar_radius > param_.map_radius) { std::stringstream message; message << "Dynamic map loading is not keeping up."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); // If the map does not keep up with the current position, @@ -110,7 +110,7 @@ bool MapUpdateModule::should_update_map( void MapUpdateModule::update_map( const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->addKeyValue("is_need_rebuild", need_rebuild_); + diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_); // If the current position is super far from the previous loading position, // lock and rebuild ndt_ptr_ @@ -130,14 +130,14 @@ void MapUpdateModule::update_map( const bool updated = update_ndt(position, *ndt_ptr_, diagnostics_ptr); // check is_updated_map - diagnostics_ptr->addKeyValue("is_updated_map", updated); + diagnostics_ptr->add_key_value("is_updated_map", updated); if (!updated) { std::stringstream message; message << "update_ndt failed. If this happens with initial position estimation, make sure that" << "(1) the initial position matches the pcd map and (2) the map_loader is working " "properly."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1000, message.str()); last_update_position_ = position; @@ -157,7 +157,7 @@ void MapUpdateModule::update_map( const bool updated = update_ndt(position, *secondary_ndt_ptr_, diagnostics_ptr); // check is_updated_map - diagnostics_ptr->addKeyValue("is_updated_map", updated); + diagnostics_ptr->add_key_value("is_updated_map", updated); if (!updated) { last_update_position_ = position; return; @@ -189,7 +189,7 @@ bool MapUpdateModule::update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, std::unique_ptr & diagnostics_ptr) { - diagnostics_ptr->addKeyValue("maps_size_before", ndt.getCurrentMapIDs().size()); + diagnostics_ptr->add_key_value("maps_size_before", ndt.getCurrentMapIDs().size()); auto request = std::make_shared(); @@ -199,11 +199,11 @@ bool MapUpdateModule::update_ndt( request->cached_ids = ndt.getCurrentMapIDs(); while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false); + diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", false); std::stringstream message; message << "Waiting for pcd loader service. Check the pointcloud_map_loader."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -217,23 +217,23 @@ bool MapUpdateModule::update_ndt( while (status != std::future_status::ready) { // check is_succeed_call_pcd_loader if (!rclcpp::ok()) { - diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", false); + diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", false); std::stringstream message; message << "pcd_loader service is not working."; - diagnostics_ptr->updateLevelAndMessage( + diagnostics_ptr->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; // No update } status = result.wait_for(std::chrono::seconds(1)); } - diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true); + diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true); auto & maps_to_add = result.get()->new_pointcloud_with_ids; auto & map_ids_to_remove = result.get()->ids_to_remove; - diagnostics_ptr->addKeyValue("maps_to_add_size", maps_to_add.size()); - diagnostics_ptr->addKeyValue("maps_to_remove_size", map_ids_to_remove.size()); + diagnostics_ptr->add_key_value("maps_to_add_size", maps_to_add.size()); + diagnostics_ptr->add_key_value("maps_to_remove_size", map_ids_to_remove.size()); if (maps_to_add.empty() && map_ids_to_remove.empty()) { return false; // No update @@ -261,9 +261,9 @@ bool MapUpdateModule::update_ndt( const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0; - diagnostics_ptr->addKeyValue("map_update_execution_time", exe_time); - diagnostics_ptr->addKeyValue("maps_size_after", ndt.getCurrentMapIDs().size()); - diagnostics_ptr->addKeyValue("is_succeed_call_pcd_loader", true); + diagnostics_ptr->add_key_value("map_update_execution_time", exe_time); + diagnostics_ptr->add_key_value("maps_size_after", ndt.getCurrentMapIDs().size()); + diagnostics_ptr->add_key_value("is_succeed_call_pcd_loader", true); return true; // Updated } 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 8acfe3bd5c1ca..e84d6709c3151 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -62,6 +62,28 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } +std::array rotate_covariance( + const std::array & src_covariance, const Eigen::Matrix3d & rotation) +{ + std::array ret_covariance = src_covariance; + + Eigen::Matrix3d src_cov; + src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], + src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], + src_covariance[14]; + + Eigen::Matrix3d ret_cov; + ret_cov = rotation * src_cov * rotation.transpose(); + + for (Eigen::Index i = 0; i < 3; ++i) { + ret_covariance[i] = ret_cov(0, i); + ret_covariance[i + 6] = ret_cov(1, i); + ret_covariance[i + 12] = ret_cov(2, i); + } + + return ret_covariance; +} + NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) : Node("ndt_scan_matcher", options), tf2_broadcaster_(*this), @@ -197,7 +219,7 @@ void NDTScanMatcher::callback_timer() diagnostics_map_update_->clear(); - diagnostics_map_update_->addKeyValue("timer_callback_time_stamp", ros_time_now.nanoseconds()); + diagnostics_map_update_->add_key_value("timer_callback_time_stamp", ros_time_now.nanoseconds()); map_update_module_->callback_timer(is_activated_, latest_ekf_position_, diagnostics_map_update_); @@ -217,16 +239,16 @@ void NDTScanMatcher::callback_initial_pose( void NDTScanMatcher::callback_initial_pose_main( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr initial_pose_msg_ptr) { - diagnostics_initial_pose_->addKeyValue( + diagnostics_initial_pose_->add_key_value( "topic_time_stamp", static_cast(initial_pose_msg_ptr->header.stamp).nanoseconds()); // check is_activated - diagnostics_initial_pose_->addKeyValue("is_activated", static_cast(is_activated_)); + diagnostics_initial_pose_->add_key_value("is_activated", static_cast(is_activated_)); if (!is_activated_) { std::stringstream message; message << "Node is not activated."; - diagnostics_initial_pose_->updateLevelAndMessage( + diagnostics_initial_pose_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -234,13 +256,13 @@ void NDTScanMatcher::callback_initial_pose_main( // check is_expected_frame_id const bool is_expected_frame_id = (initial_pose_msg_ptr->header.frame_id == param_.frame.map_frame); - diagnostics_initial_pose_->addKeyValue("is_expected_frame_id", is_expected_frame_id); + diagnostics_initial_pose_->add_key_value("is_expected_frame_id", is_expected_frame_id); if (!is_expected_frame_id) { std::stringstream message; message << "Received initial pose message with frame_id " << initial_pose_msg_ptr->header.frame_id << ", but expected " << param_.frame.map_frame << ". Please check the frame_id in the input topic and ensure it is correct."; - diagnostics_initial_pose_->updateLevelAndMessage( + diagnostics_initial_pose_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); return; } @@ -259,7 +281,7 @@ void NDTScanMatcher::callback_regularization_pose( { diagnostics_regularization_pose_->clear(); - diagnostics_regularization_pose_->addKeyValue( + diagnostics_regularization_pose_->add_key_value( "topic_time_stamp", static_cast(pose_conv_msg_ptr->header.stamp).nanoseconds()); regularization_pose_buffer_->push_back(pose_conv_msg_ptr); @@ -282,11 +304,11 @@ void NDTScanMatcher::callback_sensor_points( const size_t error_skipping_publish_num = 5; skipping_publish_num = ((is_succeed_scan_matching || !is_activated_) ? 0 : (skipping_publish_num + 1)); - diagnostics_scan_points_->addKeyValue("skipping_publish_num", skipping_publish_num); + diagnostics_scan_points_->add_key_value("skipping_publish_num", skipping_publish_num); if (skipping_publish_num >= error_skipping_publish_num) { std::stringstream message; message << "skipping_publish_num exceed limit (" << skipping_publish_num << " times)."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } @@ -300,15 +322,15 @@ bool NDTScanMatcher::callback_sensor_points_main( // check topic_time_stamp const rclcpp::Time sensor_ros_time = sensor_points_msg_in_sensor_frame->header.stamp; - diagnostics_scan_points_->addKeyValue("topic_time_stamp", sensor_ros_time.nanoseconds()); + diagnostics_scan_points_->add_key_value("topic_time_stamp", sensor_ros_time.nanoseconds()); // check sensor_points_size const size_t sensor_points_size = sensor_points_msg_in_sensor_frame->width; - diagnostics_scan_points_->addKeyValue("sensor_points_size", sensor_points_size); + diagnostics_scan_points_->add_key_value("sensor_points_size", sensor_points_size); if (sensor_points_size == 0) { std::stringstream message; message << "Sensor points is empty."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -316,14 +338,14 @@ bool NDTScanMatcher::callback_sensor_points_main( // check sensor_points_delay_time_sec const double sensor_points_delay_time_sec = (this->now() - sensor_points_msg_in_sensor_frame->header.stamp).seconds(); - diagnostics_scan_points_->addKeyValue( + diagnostics_scan_points_->add_key_value( "sensor_points_delay_time_sec", sensor_points_delay_time_sec); if (sensor_points_delay_time_sec > param_.validation.lidar_topic_timeout_sec) { std::stringstream message; message << "sensor points is experiencing latency." << "The delay time is " << sensor_points_delay_time_sec << "[sec] " << "(the tolerance is " << param_.validation.lidar_topic_timeout_sec << "[sec])."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); // If the delay time of the LiDAR topic exceeds the delay compensation time of ekf_localizer, @@ -352,13 +374,13 @@ bool NDTScanMatcher::callback_sensor_points_main( std::stringstream message; message << ex.what() << ". Please publish TF " << sensor_frame << " to " << param_.frame.base_frame; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_scan_points_->addKeyValue("is_succeed_transform_sensor_points", false); + diagnostics_scan_points_->add_key_value("is_succeed_transform_sensor_points", false); return false; } - diagnostics_scan_points_->addKeyValue("is_succeed_transform_sensor_points", true); + diagnostics_scan_points_->add_key_value("is_succeed_transform_sensor_points", true); // check sensor_points_max_distance double max_distance = 0.0; @@ -367,12 +389,12 @@ bool NDTScanMatcher::callback_sensor_points_main( max_distance = std::max(max_distance, distance); } - diagnostics_scan_points_->addKeyValue("sensor_points_max_distance", max_distance); + diagnostics_scan_points_->add_key_value("sensor_points_max_distance", max_distance); if (max_distance < param_.sensor_points.required_distance) { std::stringstream message; message << "Max distance of sensor points = " << std::fixed << std::setprecision(3) << max_distance << " [m] < " << param_.sensor_points.required_distance << " [m]"; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -384,11 +406,11 @@ bool NDTScanMatcher::callback_sensor_points_main( ndt_ptr_->setInputSource(sensor_points_in_baselink_frame); // check is_activated - diagnostics_scan_points_->addKeyValue("is_activated", static_cast(is_activated_)); + diagnostics_scan_points_->add_key_value("is_activated", static_cast(is_activated_)); if (!is_activated_) { std::stringstream message; message << "Node is not activated."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -399,12 +421,12 @@ bool NDTScanMatcher::callback_sensor_points_main( // check is_succeed_interpolate_initial_pose const bool is_succeed_interpolate_initial_pose = (interpolation_result_opt != std::nullopt); - diagnostics_scan_points_->addKeyValue( + diagnostics_scan_points_->add_key_value( "is_succeed_interpolate_initial_pose", is_succeed_interpolate_initial_pose); if (!is_succeed_interpolate_initial_pose) { std::stringstream message; message << "Couldn't interpolate pose. Please check the initial pose topic"; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -420,11 +442,11 @@ bool NDTScanMatcher::callback_sensor_points_main( // check is_set_map_points const bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); - diagnostics_scan_points_->addKeyValue("is_set_map_points", is_set_map_points); + diagnostics_scan_points_->add_key_value("is_set_map_points", is_set_map_points); if (!is_set_map_points) { std::stringstream message; message << "Map points is not set."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return false; } @@ -444,31 +466,33 @@ bool NDTScanMatcher::callback_sensor_points_main( } // check iteration_num - diagnostics_scan_points_->addKeyValue("iteration_num", ndt_result.iteration_num); + diagnostics_scan_points_->add_key_value("iteration_num", ndt_result.iteration_num); const bool is_ok_iteration_num = (ndt_result.iteration_num < ndt_ptr_->getMaximumIterations()); if (!is_ok_iteration_num) { std::stringstream message; message << "The number of iterations has reached its upper limit. The number of iterations: " << ndt_result.iteration_num << ", Limit: " << ndt_ptr_->getMaximumIterations() << "."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } // check local_optimal_solution_oscillation_num constexpr int oscillation_num_threshold = 10; const int oscillation_num = count_oscillation(transformation_msg_array); - diagnostics_scan_points_->addKeyValue("local_optimal_solution_oscillation_num", oscillation_num); + diagnostics_scan_points_->add_key_value( + "local_optimal_solution_oscillation_num", oscillation_num); const bool is_local_optimal_solution_oscillation = (oscillation_num > oscillation_num_threshold); if (is_local_optimal_solution_oscillation) { std::stringstream message; message << "There is a possibility of oscillation in a local minimum"; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } // check score - diagnostics_scan_points_->addKeyValue("transform_probability", ndt_result.transform_probability); - diagnostics_scan_points_->addKeyValue( + diagnostics_scan_points_->add_key_value( + "transform_probability", ndt_result.transform_probability); + diagnostics_scan_points_->add_key_value( "nearest_voxel_transformation_likelihood", ndt_result.nearest_voxel_transformation_likelihood); double score = 0.0; double score_threshold = 0.0; @@ -484,17 +508,49 @@ bool NDTScanMatcher::callback_sensor_points_main( } else { std::stringstream message; message << "Unknown converged param type. Please check `score_estimation.converged_param_type`"; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); return false; } + // check score diff + const std::vector & tp_array = ndt_result.transform_probability_array; + if (static_cast(tp_array.size()) != ndt_result.iteration_num + 1) { + // only publish warning to /diagnostics, not skip publishing pose + std::stringstream message; + message << "transform_probability_array size is not equal to iteration_num + 1." + << " transform_probability_array size: " << tp_array.size() + << ", iteration_num: " << ndt_result.iteration_num; + diagnostics_scan_points_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } else { + const float diff = tp_array.back() - tp_array.front(); + diagnostics_scan_points_->add_key_value("transform_probability_diff", diff); + diagnostics_scan_points_->add_key_value("transform_probability_before", tp_array.front()); + } + const std::vector & nvtl_array = ndt_result.nearest_voxel_transformation_likelihood_array; + if (static_cast(nvtl_array.size()) != ndt_result.iteration_num + 1) { + // only publish warning to /diagnostics, not skip publishing pose + std::stringstream message; + message + << "nearest_voxel_transformation_likelihood_array size is not equal to iteration_num + 1." + << " nearest_voxel_transformation_likelihood_array size: " << nvtl_array.size() + << ", iteration_num: " << ndt_result.iteration_num; + diagnostics_scan_points_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } else { + const float diff = nvtl_array.back() - nvtl_array.front(); + diagnostics_scan_points_->add_key_value("nearest_voxel_transformation_likelihood_diff", diff); + diagnostics_scan_points_->add_key_value( + "nearest_voxel_transformation_likelihood_before", nvtl_array.front()); + } + bool is_ok_score = (score > score_threshold); if (!is_ok_score) { std::stringstream message; message << "Score is below the threshold. Score: " << score << ", Threshold: " << score_threshold; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); RCLCPP_WARN_STREAM(this->get_logger(), message.str()); } @@ -521,13 +577,13 @@ bool NDTScanMatcher::callback_sensor_points_main( // check distance_initial_to_result const auto distance_initial_to_result = static_cast( norm(interpolation_result.interpolated_pose.pose.pose.position, result_pose_msg.position)); - diagnostics_scan_points_->addKeyValue("distance_initial_to_result", distance_initial_to_result); + diagnostics_scan_points_->add_key_value("distance_initial_to_result", distance_initial_to_result); const double warn_distance_initial_to_result = 3.0; if (distance_initial_to_result > warn_distance_initial_to_result) { std::stringstream message; message << "distance_initial_to_result is too large (" << distance_initial_to_result << " [m])."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } @@ -536,11 +592,11 @@ bool NDTScanMatcher::callback_sensor_points_main( const auto duration_micro_sec = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0f; - diagnostics_scan_points_->addKeyValue("execution_time", exe_time); + diagnostics_scan_points_->add_key_value("execution_time", exe_time); if (exe_time > param_.validation.critical_upper_bound_exe_time_ms) { std::stringstream message; message << "NDT exe time is too long (took " << exe_time << " [ms])."; - diagnostics_scan_points_->updateLevelAndMessage( + diagnostics_scan_points_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } @@ -612,7 +668,7 @@ void NDTScanMatcher::transform_sensor_measurement( geometry_msgs::msg::TransformStamped transform; try { transform = tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { + } catch (const tf2::TransformException & ex) { throw; } @@ -753,28 +809,6 @@ int NDTScanMatcher::count_oscillation( return max_oscillation_cnt; } -std::array NDTScanMatcher::rotate_covariance( - const std::array & src_covariance, const Eigen::Matrix3d & rotation) const -{ - std::array ret_covariance = src_covariance; - - Eigen::Matrix3d src_cov; - src_cov << src_covariance[0], src_covariance[1], src_covariance[2], src_covariance[6], - src_covariance[7], src_covariance[8], src_covariance[12], src_covariance[13], - src_covariance[14]; - - Eigen::Matrix3d ret_cov; - ret_cov = rotation * src_cov * rotation.transpose(); - - for (Eigen::Index i = 0; i < 3; ++i) { - ret_covariance[i] = ret_cov(0, i); - ret_covariance[i + 6] = ret_cov(1, i); - ret_covariance[i + 12] = ret_cov(2, i); - } - - return ret_covariance; -} - std::array NDTScanMatcher::estimate_covariance( const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time) @@ -874,7 +908,7 @@ void NDTScanMatcher::service_trigger_node( const rclcpp::Time ros_time_now = this->now(); diagnostics_trigger_node_->clear(); - diagnostics_trigger_node_->addKeyValue("service_call_time_stamp", ros_time_now.nanoseconds()); + diagnostics_trigger_node_->add_key_value("service_call_time_stamp", ros_time_now.nanoseconds()); is_activated_ = req->data; if (is_activated_) { @@ -882,8 +916,8 @@ void NDTScanMatcher::service_trigger_node( } res->success = true; - diagnostics_trigger_node_->addKeyValue("is_activated", static_cast(is_activated_)); - diagnostics_trigger_node_->addKeyValue("is_succeed_service", res->success); + diagnostics_trigger_node_->add_key_value("is_activated", static_cast(is_activated_)); + diagnostics_trigger_node_->add_key_value("is_succeed_service", res->success); diagnostics_trigger_node_->publish(ros_time_now); } @@ -895,17 +929,17 @@ void NDTScanMatcher::service_ndt_align( diagnostics_ndt_align_->clear(); - diagnostics_ndt_align_->addKeyValue("service_call_time_stamp", ros_time_now.nanoseconds()); + diagnostics_ndt_align_->add_key_value("service_call_time_stamp", ros_time_now.nanoseconds()); service_ndt_align_main(req, res); // check is_succeed_service bool is_succeed_service = res->success; - diagnostics_ndt_align_->addKeyValue("is_succeed_service", is_succeed_service); + diagnostics_ndt_align_->add_key_value("is_succeed_service", is_succeed_service); if (!is_succeed_service) { std::stringstream message; message << "ndt_align_service is failed."; - diagnostics_ndt_align_->updateLevelAndMessage( + diagnostics_ndt_align_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } @@ -928,17 +962,17 @@ void NDTScanMatcher::service_ndt_align_main( // "gnss_link" instead of "map". The ndt_align is designed to return identity when this issue // occurs. However, in the future, converting to a non-existent frame_id should be prohibited. - diagnostics_ndt_align_->addKeyValue("is_succeed_transform_initial_pose", false); + diagnostics_ndt_align_->add_key_value("is_succeed_transform_initial_pose", false); std::stringstream message; message << "Please publish TF " << target_frame.c_str() << " to " << source_frame.c_str(); - diagnostics_ndt_align_->updateLevelAndMessage( + diagnostics_ndt_align_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); res->success = false; return; } - diagnostics_ndt_align_->addKeyValue("is_succeed_transform_initial_pose", true); + diagnostics_ndt_align_->add_key_value("is_succeed_transform_initial_pose", true); // transform pose_frame to map_frame const auto initial_pose_msg_in_map_frame = transform(req->pose_with_covariance, transform_s2t); @@ -950,11 +984,11 @@ void NDTScanMatcher::service_ndt_align_main( // check is_set_map_points bool is_set_map_points = (ndt_ptr_->getInputTarget() != nullptr); - diagnostics_ndt_align_->addKeyValue("is_set_map_points", is_set_map_points); + diagnostics_ndt_align_->add_key_value("is_set_map_points", is_set_map_points); if (!is_set_map_points) { std::stringstream message; message << "No InputTarget. Please check the map file and the map_loader service"; - diagnostics_ndt_align_->updateLevelAndMessage( + diagnostics_ndt_align_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); res->success = false; @@ -963,11 +997,11 @@ void NDTScanMatcher::service_ndt_align_main( // check is_set_sensor_points bool is_set_sensor_points = (ndt_ptr_->getInputSource() != nullptr); - diagnostics_ndt_align_->addKeyValue("is_set_sensor_points", is_set_sensor_points); + diagnostics_ndt_align_->add_key_value("is_set_sensor_points", is_set_sensor_points); if (!is_set_sensor_points) { std::stringstream message; message << "No InputSource. Please check the input lidar topic"; - diagnostics_ndt_align_->updateLevelAndMessage( + diagnostics_ndt_align_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); res->success = false; @@ -1075,7 +1109,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( 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); - diagnostics_ndt_align_->addKeyValue("best_particle_score", best_particle_ptr->score); + diagnostics_ndt_align_->add_key_value("best_particle_score", best_particle_ptr->score); return result_pose_with_cov_msg; } diff --git a/localization/ndt_scan_matcher/test/test_util.hpp b/localization/ndt_scan_matcher/test/test_util.hpp index 2f78827abf891..dbf055d3c95d3 100644 --- a/localization/ndt_scan_matcher/test/test_util.hpp +++ b/localization/ndt_scan_matcher/test/test_util.hpp @@ -15,8 +15,13 @@ #ifndef TEST_UTIL_HPP_ #define TEST_UTIL_HPP_ +#include +#include + +#include #include #include +#include inline pcl::PointCloud make_sample_half_cubic_pcd() { diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index 2a586aa9cd049..ee63d9f43559a 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/localization/pose2twist/CMakeLists.txt @@ -14,6 +14,13 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTOR SingleThreadedExecutor ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_angular_velocity + test/test_angular_velocity.cpp + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/pose2twist/README.md b/localization/pose2twist/README.md index 07d9c37b710fc..f1f7d6408fafb 100644 --- a/localization/pose2twist/README.md +++ b/localization/pose2twist/README.md @@ -5,7 +5,7 @@ This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging. The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero. -The `twist.angular` is calculated as `d_roll / dt`, `d_pitch / dt` and `d_yaw / dt` for each field. +The `twist.angular` is calculated as `relative_rotation_vector / dt` for each field. ## Inputs / Outputs diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index 459a62ea5cd13..d1ff6ee5ff8b6 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -21,14 +21,23 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +// Compute the relative rotation of q2 from q1 as a rotation vector +geometry_msgs::msg::Vector3 compute_relative_rotation_vector( + const tf2::Quaternion & q1, const tf2::Quaternion & q2); + class Pose2Twist : public rclcpp::Node { public: explicit Pose2Twist(const rclcpp::NodeOptions & options); - ~Pose2Twist() = default; private: - void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr); + void callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr); rclcpp::Subscription::SharedPtr pose_sub_; diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index 4b98ec6c81ad4..cdde78ed7e357 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -14,12 +14,6 @@ #include "pose2twist/pose2twist_core.hpp" -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include @@ -38,35 +32,27 @@ Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose create_publisher("angular_z", durable_qos); // Note: this callback publishes topics above pose_sub_ = create_subscription( - "pose", queue_size, std::bind(&Pose2Twist::callbackPose, this, _1)); -} - -double calcDiffForRadian(const double lhs_rad, const double rhs_rad) -{ - double diff_rad = lhs_rad - rhs_rad; - if (diff_rad > M_PI) { - diff_rad = diff_rad - 2 * M_PI; - } else if (diff_rad < -M_PI) { - diff_rad = diff_rad + 2 * M_PI; - } - return diff_rad; + "pose", queue_size, std::bind(&Pose2Twist::callback_pose, this, _1)); } -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) +tf2::Quaternion get_quaternion(const geometry_msgs::msg::PoseStamped::SharedPtr & pose_stamped_ptr) { - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; + const auto & orientation = pose_stamped_ptr->pose.orientation; + return tf2::Quaternion{orientation.x, orientation.y, orientation.z, orientation.w}; } -geometry_msgs::msg::Vector3 getRPY(geometry_msgs::msg::PoseStamped::SharedPtr pose) +geometry_msgs::msg::Vector3 compute_relative_rotation_vector( + const tf2::Quaternion & q1, const tf2::Quaternion & q2) { - return getRPY(pose->pose); + // If we define q2 as the rotation obtained by applying dq after applying q1, + // then q2 = q1 * dq . + // Therefore, dq = q1.inverse() * q2 . + const tf2::Quaternion diff_quaternion = q1.inverse() * q2; + const tf2::Vector3 axis = diff_quaternion.getAxis() * diff_quaternion.getAngle(); + return geometry_msgs::msg::Vector3{}.set__x(axis.x()).set__y(axis.y()).set__z(axis.z()); } -geometry_msgs::msg::TwistStamped calcTwist( +geometry_msgs::msg::TwistStamped calc_twist( geometry_msgs::msg::PoseStamped::SharedPtr pose_a, geometry_msgs::msg::PoseStamped::SharedPtr pose_b) { @@ -79,18 +65,16 @@ geometry_msgs::msg::TwistStamped calcTwist( return twist; } - const auto pose_a_rpy = getRPY(pose_a); - const auto pose_b_rpy = getRPY(pose_b); + const auto pose_a_quaternion = get_quaternion(pose_a); + const auto pose_b_quaternion = get_quaternion(pose_b); geometry_msgs::msg::Vector3 diff_xyz; - geometry_msgs::msg::Vector3 diff_rpy; + const geometry_msgs::msg::Vector3 relative_rotation_vector = + compute_relative_rotation_vector(pose_a_quaternion, pose_b_quaternion); diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x; diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y; diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z; - diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x); - diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y); - diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z); geometry_msgs::msg::TwistStamped twist; twist.header = pose_b->header; @@ -99,34 +83,34 @@ geometry_msgs::msg::TwistStamped calcTwist( dt; twist.twist.linear.y = 0; twist.twist.linear.z = 0; - twist.twist.angular.x = diff_rpy.x / dt; - twist.twist.angular.y = diff_rpy.y / dt; - twist.twist.angular.z = diff_rpy.z / dt; + twist.twist.angular.x = relative_rotation_vector.x / dt; + twist.twist.angular.y = relative_rotation_vector.y / dt; + twist.twist.angular.z = relative_rotation_vector.z / dt; return twist; } -void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr) +void Pose2Twist::callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr) { // TODO(YamatoAndo) check time stamp diff // TODO(YamatoAndo) check suddenly move // TODO(YamatoAndo) apply low pass filter - geometry_msgs::msg::PoseStamped::SharedPtr current_pose_msg = pose_msg_ptr; + const geometry_msgs::msg::PoseStamped::SharedPtr & current_pose_msg = pose_msg_ptr; static geometry_msgs::msg::PoseStamped::SharedPtr prev_pose_msg = current_pose_msg; - geometry_msgs::msg::TwistStamped twist_msg = calcTwist(prev_pose_msg, current_pose_msg); + geometry_msgs::msg::TwistStamped twist_msg = calc_twist(prev_pose_msg, current_pose_msg); prev_pose_msg = current_pose_msg; twist_msg.header.frame_id = "base_link"; twist_pub_->publish(twist_msg); tier4_debug_msgs::msg::Float32Stamped linear_x_msg; linear_x_msg.stamp = this->now(); - linear_x_msg.data = twist_msg.twist.linear.x; + linear_x_msg.data = static_cast(twist_msg.twist.linear.x); linear_x_pub_->publish(linear_x_msg); tier4_debug_msgs::msg::Float32Stamped angular_z_msg; angular_z_msg.stamp = this->now(); - angular_z_msg.data = twist_msg.twist.angular.z; + angular_z_msg.data = static_cast(twist_msg.twist.angular.z); angular_z_pub_->publish(angular_z_msg); } diff --git a/localization/pose2twist/test/test_angular_velocity.cpp b/localization/pose2twist/test/test_angular_velocity.cpp new file mode 100644 index 0000000000000..bf2ca0a3ba5c2 --- /dev/null +++ b/localization/pose2twist/test/test_angular_velocity.cpp @@ -0,0 +1,115 @@ +// Copyright 2022 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 "pose2twist/pose2twist_core.hpp" + +#include + +// 1e-3 radian = 0.057 degrees +constexpr double acceptable_error = 1e-3; + +TEST(AngularVelocityFromQuaternion, CheckMultiplicationOrder) +{ + // If we define q2 as the rotation obtained by applying dq after applying q1, then q2 = q1 * dq . + // + // IT IS NOT q2 = dq * q1 . + // + // This test checks that the multiplication order is correct. + + const tf2::Vector3 target_vector(1, 2, 3); + // initial state + // Now, car is facing to the +x direction + // z + // ^ y + // | ^ + // | / + // | / + // car -> x + // + // + // + + tf2::Quaternion q1; + q1.setRPY(0., 0., M_PI / 2.); // yaw = 90 degrees + const tf2::Vector3 initially_rotated_vector = tf2::quatRotate(q1, target_vector); + // after applying q1 + // Now, car is facing to the +y direction + // z + // ^ + // | y + // | ^ + // | / + // <--car x + // + // + // + EXPECT_NEAR(initially_rotated_vector.x(), -2., acceptable_error); + EXPECT_NEAR(initially_rotated_vector.y(), 1., acceptable_error); + EXPECT_NEAR(initially_rotated_vector.z(), 3., acceptable_error); + + tf2::Quaternion dq; + dq.setRPY(0., M_PI / 2., 0.); // pitch = 90 degrees + const tf2::Vector3 finally_rotated_vector = tf2::quatRotate(q1 * dq, target_vector); + // after applying dq + // Now, car is facing to the -z direction + // z y + // ^ + // / + // / + // / + // <--car x + // | + // v + // + EXPECT_NEAR(finally_rotated_vector.x(), -2., acceptable_error); + EXPECT_NEAR(finally_rotated_vector.y(), 3., acceptable_error); + EXPECT_NEAR(finally_rotated_vector.z(), -1., acceptable_error); + + // Failure case + { + const tf2::Vector3 false_rotated_vector = tf2::quatRotate(dq * q1, target_vector); + + EXPECT_FALSE(std::abs(false_rotated_vector.x() - (-2)) < acceptable_error); + EXPECT_FALSE(std::abs(false_rotated_vector.y() - (3)) < acceptable_error); + EXPECT_FALSE(std::abs(false_rotated_vector.z() - (-1)) < acceptable_error); + } +} + +TEST(AngularVelocityFromQuaternion, CheckNumericalValidity) +{ + auto test = [](const tf2::Vector3 & expected_axis, const double expected_angle) -> void { + tf2::Quaternion expected_q; + expected_q.setRotation(expected_axis, expected_angle); + + // Create a random initial quaternion + tf2::Quaternion initial_q; + initial_q.setRPY(0.2, 0.3, 0.4); + + // Calculate the final quaternion by rotating the initial quaternion by the expected + // quaternion + const tf2::Quaternion final_q = initial_q * expected_q; + + // Calculate the relative rotation between the initial and final quaternion + const geometry_msgs::msg::Vector3 rotation_vector = + compute_relative_rotation_vector(initial_q, final_q); + + EXPECT_NEAR(rotation_vector.x, expected_axis.x() * expected_angle, acceptable_error); + EXPECT_NEAR(rotation_vector.y, expected_axis.y() * expected_angle, acceptable_error); + EXPECT_NEAR(rotation_vector.z, expected_axis.z() * expected_angle, acceptable_error); + }; + + test(tf2::Vector3(1.0, 0.0, 0.0).normalized(), 0.1); // 0.1 radian = 5.7 degrees + test(tf2::Vector3(1.0, 1.0, 0.0).normalized(), -0.2); // 0.2 radian = 11.4 degrees + test(tf2::Vector3(1.0, 2.0, 3.0).normalized(), 0.3); // 0.3 radian = 17.2 degrees +} diff --git a/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp b/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp index 1274092ae4993..33d95c58a726b 100644 --- a/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp +++ b/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp @@ -40,7 +40,7 @@ template std::array get_covariance_parameter(NodeT * node, const std::string & name) { const auto parameter = node->template declare_parameter>(name); - std::array covariance; + std::array covariance{}; copy_vector_to_array(parameter, covariance); return covariance; } diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.cpp b/localization/pose_initializer/src/pose_initializer/gnss_module.cpp index e8762c8c6f493..b746828e33b1c 100644 --- a/localization/pose_initializer/src/pose_initializer/gnss_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/gnss_module.cpp @@ -19,13 +19,18 @@ #include -GnssModule::GnssModule(rclcpp::Node * node) : fitter_(node) +GnssModule::GnssModule(rclcpp::Node * node) +: fitter_(node), + clock_(node->get_clock()), + timeout_(node->declare_parameter("gnss_pose_timeout")) { sub_gnss_pose_ = node->create_subscription( - "gnss_pose_cov", 1, [this](PoseWithCovarianceStamped::ConstSharedPtr msg) { pose_ = msg; }); + "gnss_pose_cov", 1, std::bind(&GnssModule::on_pose, this, std::placeholders::_1)); +} - clock_ = node->get_clock(); - timeout_ = node->declare_parameter("gnss_pose_timeout"); +void GnssModule::on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + pose_ = msg; } geometry_msgs::msg::PoseWithCovarianceStamped GnssModule::get_pose() diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp index 8c3bc658e7e7a..fd490b00d0f70 100644 --- a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp @@ -30,6 +30,8 @@ class GnssModule PoseWithCovarianceStamped get_pose(); private: + void on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg); + map_height_fitter::MapHeightFitter fitter_; rclcpp::Clock::SharedPtr clock_; rclcpp::Subscription::SharedPtr sub_gnss_pose_; diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp index 6e11f8fe74212..1f657545e687c 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp @@ -23,9 +23,9 @@ using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -NdtModule::NdtModule(rclcpp::Node * node) : logger_(node->get_logger()) +NdtModule::NdtModule(rclcpp::Node * node) +: logger_(node->get_logger()), cli_align_(node->create_client("ndt_align")) { - cli_align_ = node->create_client("ndt_align"); } PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped & pose) diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 2fc7d61890837..1111547b393e7 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -67,28 +67,24 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) throw std::invalid_argument( "Could not set user defined initial pose. The size of initial_pose is " + std::to_string(initial_pose_array.size()) + ". It must be 7."); - } else if ( + } + if ( std::abs(initial_pose_array[3]) < 1e-6 && std::abs(initial_pose_array[4]) < 1e-6 && std::abs(initial_pose_array[5]) < 1e-6 && std::abs(initial_pose_array[6]) < 1e-6) { throw std::invalid_argument("Input quaternion is invalid. All elements are close to zero."); - } else { - geometry_msgs::msg::Pose initial_pose; - initial_pose.position.x = initial_pose_array[0]; - initial_pose.position.y = initial_pose_array[1]; - initial_pose.position.z = initial_pose_array[2]; - initial_pose.orientation.x = initial_pose_array[3]; - initial_pose.orientation.y = initial_pose_array[4]; - initial_pose.orientation.z = initial_pose_array[5]; - initial_pose.orientation.w = initial_pose_array[6]; - - set_user_defined_initial_pose(initial_pose, true); } - } -} -PoseInitializer::~PoseInitializer() -{ - // to delete gnss module + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = initial_pose_array[0]; + initial_pose.position.y = initial_pose_array[1]; + initial_pose.position.z = initial_pose_array[2]; + initial_pose.orientation.x = initial_pose_array[3]; + initial_pose.orientation.y = initial_pose_array[4]; + initial_pose.orientation.z = initial_pose_array[5]; + initial_pose.orientation.w = initial_pose_array[6]; + + set_user_defined_initial_pose(initial_pose, true); + } } void PoseInitializer::change_state(State::Message::_state_type state) @@ -175,7 +171,7 @@ void PoseInitializer::on_initialize( std::stringstream message; message << "No input pose_with_covariance. If you want to use DIRECT method, please input " "pose_with_covariance."; - RCLCPP_ERROR(get_logger(), message.str().c_str()); + RCLCPP_ERROR_STREAM(get_logger(), message.str()); throw ServiceException( autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); } @@ -186,7 +182,7 @@ void PoseInitializer::on_initialize( } else { std::stringstream message; message << "Unknown method type (=" << std::to_string(req->method) << ")"; - RCLCPP_ERROR(get_logger(), message.str().c_str()); + RCLCPP_ERROR_STREAM(get_logger(), message.str()); throw ServiceException( autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); } diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index eeba71b90129b..d80fb26fdd41f 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -35,7 +35,6 @@ class PoseInitializer : public rclcpp::Node { public: explicit PoseInitializer(const rclcpp::NodeOptions & options); - ~PoseInitializer(); private: using ServiceException = component_interface_utils::ServiceException; @@ -48,8 +47,8 @@ class PoseInitializer : public rclcpp::Node component_interface_utils::Publisher::SharedPtr pub_state_; component_interface_utils::Service::SharedPtr srv_initialize_; State::Message state_; - std::array output_pose_covariance_; - std::array gnss_particle_covariance_; + std::array output_pose_covariance_{}; + std::array gnss_particle_covariance_{}; std::unique_ptr gnss_; std::unique_ptr ndt_; std::unique_ptr yabloc_; diff --git a/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp b/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp index f79f1e58e8d62..204f2289adbf4 100644 --- a/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp @@ -23,9 +23,9 @@ using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -YabLocModule::YabLocModule(rclcpp::Node * node) : logger_(node->get_logger()) +YabLocModule::YabLocModule(rclcpp::Node * node) +: logger_(node->get_logger()), cli_align_(node->create_client("yabloc_align")) { - cli_align_ = node->create_client("yabloc_align"); } PoseWithCovarianceStamped YabLocModule::align_pose(const PoseWithCovarianceStamped & pose) diff --git a/localization/pose_initializer/test/test_copy_vector_to_array.cpp b/localization/pose_initializer/test/test_copy_vector_to_array.cpp index 65b1d7e7d2711..f372a40076ffb 100644 --- a/localization/pose_initializer/test/test_copy_vector_to_array.cpp +++ b/localization/pose_initializer/test/test_copy_vector_to_array.cpp @@ -19,7 +19,7 @@ TEST(CopyVectorToArray, CopyAllElements) { const std::vector vector{0, 1, 2, 3, 4}; - std::array array; + std::array array{}; copy_vector_to_array(vector, array); EXPECT_THAT(array, testing::ElementsAre(0, 1, 2, 3, 4)); } @@ -28,7 +28,7 @@ TEST(CopyVectorToArray, CopyZeroElements) { const std::vector vector{}; // just confirm that this works - std::array array; + std::array array{}; copy_vector_to_array(vector, array); } @@ -36,7 +36,7 @@ TEST(CopyVectorToArray, ThrowsInvalidArgumentIfMoreElementsExpected) { auto f = [] { const std::vector vector{0, 1, 2, 3, 4}; - std::array array; + std::array array{}; copy_vector_to_array(vector, array); }; diff --git a/localization/pose_instability_detector/README.md b/localization/pose_instability_detector/README.md index 89cf6ca3be684..4ced0fa8eb97b 100644 --- a/localization/pose_instability_detector/README.md +++ b/localization/pose_instability_detector/README.md @@ -1,20 +1,111 @@ # pose_instability_detector -The `pose_instability_detector` package includes a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). +The `pose_instability_detector` is a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). This node triggers periodic timer callbacks to compare two poses: -- The pose obtained by integrating the twist values from the last received message on `/localization/kinematic_state` over a duration specified by `interval_sec`. +- The pose calculated by dead reckoning starting from the pose of `/localization/kinematic_state` obtained `timer_period` seconds ago. - The latest pose from `/localization/kinematic_state`. The results of this comparison are then output to the `/diagnostics` topic. +![overview](./media/pose_instability_detector_overview.png) + +![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) + If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values. +In other words, WARN outputs indicate that the vehicle has moved to a place outside the expected range based on the twist values. This discrepancy suggests that there may be an issue with either the estimated pose or the input twist. -The following diagram provides an overview of what the timeline of this process looks like: +The following diagram provides an overview of how the procedure looks like: + +![procedure](./media/pose_instabilty_detector_procedure.svg) + +## Dead reckoning algorithm + +Dead reckoning is a method of estimating the position of a vehicle based on its previous position and velocity. +The procedure for dead reckoning is as follows: + +1. Capture the necessary twist values from the `/input/twist` topic. +2. Integrate the twist values to calculate the pose transition. +3. Apply the pose transition to the previous pose to obtain the current pose. + +### Collecting twist values + +The `pose_instability_detector` node collects the twist values from the `~/input/twist` topic to perform dead reckoning. +Ideally, `pose_instability_detector` needs the twist values between the previous pose and the current pose. +Therefore, `pose_instability_detector` snips the twist buffer and apply interpolations and extrapolations to obtain the twist values at the desired time. + +![how_to_snip_necessary_twist](./media/how_to_snip_twist.png) + +### Linear transition and angular transition + +After the twist values are collected, the node calculates the linear transition and angular transition based on the twist values and add them to the previous pose. + +## Threshold definition + +The `pose_instability_detector` node compares the pose calculated by dead reckoning with the latest pose from the EKF output. +These two pose are ideally the same, but in reality, they are not due to the error in the twist values the pose observation. +If these two poses are significantly different so that the absolute value exceeds the threshold, the node outputs a WARN message to the `/diagnostics` topic. +There are six thresholds (x, y, z, roll, pitch, and yaw) to determine whether the poses are significantly different, and these thresholds are determined by the following subsections. + +### `diff_position_x` + +This threshold examines the difference in the longitudinal axis between the two poses, and check whether the vehicle goes beyond the expected error. +This threshold is a sum of "maximum longitudinal error due to velocity scale factor error" and "pose estimation error tolerance". + +$$ +\tau_x = v_{\rm max}\frac{\beta_v}{100} \Delta t + \epsilon_x\\ +$$ -![timeline](./media/timeline.drawio.svg) +| Symbol | Description | Unit | +| ------------- | -------------------------------------------------------------------------------- | ----- | +| $\tau_x$ | Threshold for the difference in the longitudinal axis | $m$ | +| $v_{\rm max}$ | Maximum velocity | $m/s$ | +| $\beta_v$ | Scale factor tolerance for the maximum velocity | $\%$ | +| $\Delta t$ | Time interval | $s$ | +| $\epsilon_x$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the longitudinal axis | $m$ | + +### `diff_position_y` and `diff_position_z` + +These thresholds examine the difference in the lateral and vertical axes between the two poses, and check whether the vehicle goes beyond the expected error. +The `pose_instability_detector` calculates the possible range where the vehicle goes, and get the maximum difference between the nominal dead reckoning pose and the maximum limit pose. + +![lateral_threshold_calculation](./media/lateral_threshold_calculation.png) + +Addition to this, the `pose_instability_detector` node considers the pose estimation error tolerance to determine the threshold. + +$$ +\tau_y = l + \epsilon_y +$$ + +| Symbol | Description | Unit | +| ------------ | ----------------------------------------------------------------------------------------------- | ---- | +| $\tau_y$ | Threshold for the difference in the lateral axis | $m$ | +| $l$ | Maximum lateral distance described in the image above (See the appendix how this is calculated) | $m$ | +| $\epsilon_y$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the lateral axis | $m$ | + +Note that `pose_instability_detector` sets the threshold for the vertical axis as the same as the lateral axis. Only the pose estimator error tolerance is different. + +### `diff_angle_x`, `diff_angle_y`, and `diff_angle_z` + +These thresholds examine the difference in the roll, pitch, and yaw angles between the two poses. +This threshold is a sum of "maximum angular error due to velocity scale factor error and bias error" and "pose estimation error tolerance". + +$$ +\tau_\phi = \tau_\theta = \tau_\psi = \left(\omega_{\rm max}\frac{\beta_\omega}{100} + b \right) \Delta t + \epsilon_\psi +$$ + +| Symbol | Description | Unit | +| ------------------ | ------------------------------------------------------------------------ | ------------- | +| $\tau_\phi$ | Threshold for the difference in the roll angle | ${\rm rad}$ | +| $\tau_\theta$ | Threshold for the difference in the pitch angle | ${\rm rad}$ | +| $\tau_\psi$ | Threshold for the difference in the yaw angle | ${\rm rad}$ | +| $\omega_{\rm max}$ | Maximum angular velocity | ${\rm rad}/s$ | +| $\beta_\omega$ | Scale factor tolerance for the maximum angular velocity | $\%$ | +| $b$ | Bias tolerance of the angular velocity | ${\rm rad}/s$ | +| $\Delta t$ | Time interval | $s$ | +| $\epsilon_\psi$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the yaw angle | ${\rm rad}$ | ## Parameters @@ -34,4 +125,44 @@ The following diagram provides an overview of what the timeline of this process | `~/debug/diff_pose` | geometry_msgs::msg::PoseStamped | diff_pose | | `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Diagnostics | -![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) +## Appendix + +On calculating the maximum lateral distance $l$, the `pose_instability_detector` node will estimate the following poses. + +| Pose | heading velocity $v$ | angular velocity $\omega$ | +| ------------------------------- | ------------------------------------------------ | -------------------------------------------------------------- | +| Nominal dead reckoning pose | $v_{\rm max}$ | $\omega_{\rm max}$ | +| Dead reckoning pose of corner A | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ | +| Dead reckoning pose of corner B | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ | +| Dead reckoning pose of corner C | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ | +| Dead reckoning pose of corner D | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ | + +Given a heading velocity $v$ and $\omega$, the 2D theoretical variation seen from the previous pose is calculated as follows: + +$$ +\begin{align*} +\left[ + \begin{matrix} + \Delta x\\ + \Delta y + \end{matrix} +\right] +&= +\left[ + \begin{matrix} + \int_{0}^{\Delta t} v \cos(\omega t) dt\\ + \int_{0}^{\Delta t} v \sin(\omega t) dt + \end{matrix} +\right] +\\ +&= +\left[ + \begin{matrix} + \frac{v}{\omega} \sin(\omega \Delta t)\\ + \frac{v}{\omega} \left(1 - \cos(\omega \Delta t)\right) + \end{matrix} +\right] +\end{align*} +$$ + +We calculate this variation for each corner and get the maximum value of the lateral distance $l$ by comparing the distance between the nominal dead reckoning pose and the corner poses. diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml index d94de020a4a12..d9b11b78885c9 100644 --- a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml +++ b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml @@ -1,9 +1,15 @@ /**: ros__parameters: - interval_sec: 0.5 # [sec] - threshold_diff_position_x: 1.0 # [m] - threshold_diff_position_y: 1.0 # [m] - threshold_diff_position_z: 1.0 # [m] - threshold_diff_angle_x: 1.0 # [rad] - threshold_diff_angle_y: 1.0 # [rad] - threshold_diff_angle_z: 1.0 # [rad] + timer_period: 0.5 # [sec] + + heading_velocity_maximum: 16.667 # [m/s] + heading_velocity_scale_factor_tolerance: 3.0 # [%] + + angular_velocity_maximum: 0.523 # [rad/s] + angular_velocity_scale_factor_tolerance: 0.2 # [%] + angular_velocity_bias_tolerance: 0.00698 # [rad/s] + + pose_estimator_longitudinal_tolerance: 0.11 # [m] + pose_estimator_lateral_tolerance: 0.11 # [m] + pose_estimator_vertical_tolerance: 0.11 # [m] + pose_estimator_angular_tolerance: 0.0175 # [rad] diff --git a/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp new file mode 100644 index 0000000000000..0a55a5005dde1 --- /dev/null +++ b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp @@ -0,0 +1,99 @@ +// 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 AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ +#define AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +class PoseInstabilityDetector : public rclcpp::Node +{ + using Quaternion = geometry_msgs::msg::Quaternion; + using Twist = geometry_msgs::msg::Twist; + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Odometry = nav_msgs::msg::Odometry; + using KeyValue = diagnostic_msgs::msg::KeyValue; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + struct ThresholdValues + { + double position_x; + double position_y; + double position_z; + double angle_x; + double angle_y; + double angle_z; + }; + + explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ThresholdValues calculate_threshold(double interval_sec); + void dead_reckon( + PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, + const std::deque & twist_deque, Pose::SharedPtr & estimated_pose); + +private: + void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr); + void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + void callback_timer(); + + std::deque clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time); + + // subscribers and timer + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::TimerBase::SharedPtr timer_; + + // publisher + rclcpp::Publisher::SharedPtr diff_pose_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + // parameters + const double timer_period_; // [sec] + + ThresholdValues threshold_values_; + + const double heading_velocity_maximum_; // [m/s] + const double heading_velocity_scale_factor_tolerance_; // [%] + + const double angular_velocity_maximum_; // [rad/s] + const double angular_velocity_scale_factor_tolerance_; // [%] + const double angular_velocity_bias_tolerance_; // [rad/s] + + const double pose_estimator_longitudinal_tolerance_; // [m] + const double pose_estimator_lateral_tolerance_; // [m] + const double pose_estimator_vertical_tolerance_; // [m] + const double pose_estimator_angular_tolerance_; // [rad] + + // variables + std::optional latest_odometry_ = std::nullopt; + std::optional prev_odometry_ = std::nullopt; + std::deque twist_buffer_; +}; + +#endif // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/media/how_to_snip_twist.png b/localization/pose_instability_detector/media/how_to_snip_twist.png new file mode 100644 index 0000000000000..6dc66a480e769 Binary files /dev/null and b/localization/pose_instability_detector/media/how_to_snip_twist.png differ diff --git a/localization/pose_instability_detector/media/lateral_threshold_calculation.png b/localization/pose_instability_detector/media/lateral_threshold_calculation.png new file mode 100644 index 0000000000000..cd919cdcb0383 Binary files /dev/null and b/localization/pose_instability_detector/media/lateral_threshold_calculation.png differ diff --git a/localization/pose_instability_detector/media/pose_instability_detector_overview.png b/localization/pose_instability_detector/media/pose_instability_detector_overview.png new file mode 100644 index 0000000000000..ea8f7a81700ae Binary files /dev/null and b/localization/pose_instability_detector/media/pose_instability_detector_overview.png differ diff --git a/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg new file mode 100644 index 0000000000000..ba45b1f52600b --- /dev/null +++ b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg @@ -0,0 +1,316 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Get necessary subsequence +
+ from twist buffer +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ Update +
+ + latest pose +
+ +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ Calculate integration of +
+ the twist subsequence +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
+ Calculate relative difference between +
+
+
+ + latest pose +
+ +
+
and
+
+ previous pose + twist integration +
+
+
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
+ Compare pose difference with thresholds +
+
+
+ Output results as + /diagnostics +
+
+
+
+
+ + + + + + + + + + + + +
+
+
+
+ + previous pose ← latest pose +
+ +
+
+
+
+ + + + + + + + + + + + +
+
+
+ Timer Callback +
+
+
+ + + + + + + + + + + + +
+
+
+ Odometry Subscription Callback +
+
+
+ + + + + + + diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/pose_instability_detector/media/rqt_runtime_monitor.png index b3ad402e48ba7..9bad665db17e5 100644 Binary files a/localization/pose_instability_detector/media/rqt_runtime_monitor.png and b/localization/pose_instability_detector/media/rqt_runtime_monitor.png differ diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json index 560d39a2d5bff..53380f8e7f252 100644 --- a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json +++ b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json @@ -1,62 +1,83 @@ { "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for Pose Instability Detector Nodes", + "title": "Parameters for Pose Instability Detector Node", "type": "object", "definitions": { "pose_instability_detector_node": { "type": "object", "properties": { - "interval_sec": { + "timer_period": { "type": "number", "default": 0.5, "exclusiveMinimum": 0, - "description": "The interval of timer_callback in seconds." + "description": "The period of timer_callback (sec)." }, - "threshold_diff_position_x": { + "heading_velocity_maximum": { "type": "number", - "default": 1.0, + "default": 16.667, "minimum": 0.0, - "description": "The threshold of diff_position x (m)." + "description": "The maximum of heading velocity (m/s)." }, - "threshold_diff_position_y": { + "heading_velocity_scale_factor_tolerance": { "type": "number", - "default": 1.0, + "default": 3.0, "minimum": 0.0, - "description": "The threshold of diff_position y (m)." + "description": "The tolerance of heading velocity scale factor (%)." }, - "threshold_diff_position_z": { + "angular_velocity_maximum": { "type": "number", - "default": 1.0, + "default": 0.523, "minimum": 0.0, - "description": "The threshold of diff_position z (m)." + "description": "The maximum of angular velocity (rad/s)." }, - "threshold_diff_angle_x": { + "angular_velocity_scale_factor_tolerance": { "type": "number", - "default": 1.0, + "default": 0.2, "minimum": 0.0, - "description": "The threshold of diff_angle x (rad)." + "description": "The tolerance of angular velocity scale factor (%)." }, - "threshold_diff_angle_y": { + "angular_velocity_bias_tolerance": { "type": "number", - "default": 1.0, + "default": 0.00698, "minimum": 0.0, - "description": "The threshold of diff_angle y (rad)." + "description": "The tolerance of angular velocity bias (rad/s)." }, - "threshold_diff_angle_z": { + "pose_estimator_longitudinal_tolerance": { "type": "number", - "default": 1.0, + "default": 0.11, "minimum": 0.0, - "description": "The threshold of diff_angle z (rad)." + "description": "The tolerance of longitudinal position of pose estimator (m)." + }, + "pose_estimator_lateral_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of lateral position of pose estimator (m)." + }, + "pose_estimator_vertical_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of vertical position of pose estimator (m)." + }, + "pose_estimator_angular_tolerance": { + "type": "number", + "default": 0.0175, + "minimum": 0.0, + "description": "The tolerance of roll angle of pose estimator (rad)." } }, "required": [ - "interval_sec", - "threshold_diff_position_x", - "threshold_diff_position_y", - "threshold_diff_position_z", - "threshold_diff_angle_x", - "threshold_diff_angle_y", - "threshold_diff_angle_z" + "timer_period", + "heading_velocity_maximum", + "heading_velocity_scale_factor_tolerance", + "angular_velocity_maximum", + "angular_velocity_scale_factor_tolerance", + "angular_velocity_bias_tolerance", + "pose_estimator_longitudinal_tolerance", + "pose_estimator_lateral_tolerance", + "pose_estimator_vertical_tolerance", + "pose_estimator_angular_tolerance" ] } }, diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index c2bce6a3db288..fa7b2ecf16562 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_instability_detector.hpp" +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -20,17 +20,32 @@ #include +#include +#include +#include #include PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) : rclcpp::Node("pose_instability_detector", options), - threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), - threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), - threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), - threshold_diff_angle_x_(this->declare_parameter("threshold_diff_angle_x")), - threshold_diff_angle_y_(this->declare_parameter("threshold_diff_angle_y")), - threshold_diff_angle_z_(this->declare_parameter("threshold_diff_angle_z")) + timer_period_(this->declare_parameter("timer_period")), + heading_velocity_maximum_(this->declare_parameter("heading_velocity_maximum")), + heading_velocity_scale_factor_tolerance_( + this->declare_parameter("heading_velocity_scale_factor_tolerance")), + angular_velocity_maximum_(this->declare_parameter("angular_velocity_maximum")), + angular_velocity_scale_factor_tolerance_( + this->declare_parameter("angular_velocity_scale_factor_tolerance")), + angular_velocity_bias_tolerance_( + this->declare_parameter("angular_velocity_bias_tolerance")), + pose_estimator_longitudinal_tolerance_( + this->declare_parameter("pose_estimator_longitudinal_tolerance")), + pose_estimator_lateral_tolerance_( + this->declare_parameter("pose_estimator_lateral_tolerance")), + pose_estimator_vertical_tolerance_( + this->declare_parameter("pose_estimator_vertical_tolerance")), + pose_estimator_angular_tolerance_( + this->declare_parameter("pose_estimator_angular_tolerance")) { + // Define subscribers, publishers and a timer. odometry_sub_ = this->create_subscription( "~/input/odometry", 10, std::bind(&PoseInstabilityDetector::callback_odometry, this, std::placeholders::_1)); @@ -39,9 +54,8 @@ PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & opt "~/input/twist", 10, std::bind(&PoseInstabilityDetector::callback_twist, this, std::placeholders::_1)); - const double interval_sec = this->declare_parameter("interval_sec"); timer_ = rclcpp::create_timer( - this, this->get_clock(), std::chrono::duration(interval_sec), + this, this->get_clock(), std::chrono::duration(timer_period_), std::bind(&PoseInstabilityDetector::callback_timer, this)); diff_pose_pub_ = this->create_publisher("~/debug/diff_pose", 10); @@ -61,6 +75,7 @@ void PoseInstabilityDetector::callback_twist( void PoseInstabilityDetector::callback_timer() { + // odometry callback and timer callback has to be called at least once if (latest_odometry_ == std::nullopt) { return; } @@ -69,6 +84,16 @@ void PoseInstabilityDetector::callback_timer() return; } + // twist callback has to be called at least once + if (twist_buffer_.empty()) { + return; + } + + // time variables + const rclcpp::Time latest_odometry_time = rclcpp::Time(latest_odometry_->header.stamp); + const rclcpp::Time prev_odometry_time = rclcpp::Time(prev_odometry_->header.stamp); + + // define lambda function to convert quaternion to rpy auto quat_to_rpy = [](const Quaternion & quat) { tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); tf2::Matrix3x3 mat(tf2_quat); @@ -79,70 +104,48 @@ void PoseInstabilityDetector::callback_timer() return std::make_tuple(roll, pitch, yaw); }; - Pose pose = prev_odometry_->pose.pose; - rclcpp::Time prev_time = rclcpp::Time(prev_odometry_->header.stamp); - for (const TwistWithCovarianceStamped & twist_with_cov : twist_buffer_) { - const Twist twist = twist_with_cov.twist.twist; - - const rclcpp::Time curr_time = rclcpp::Time(twist_with_cov.header.stamp); - if (curr_time > latest_odometry_->header.stamp) { + // delete twist data older than prev_odometry_ (but preserve the one right before prev_odometry_) + while (twist_buffer_.size() > 1) { + if (rclcpp::Time(twist_buffer_[1].header.stamp) < prev_odometry_time) { + twist_buffer_.pop_front(); + } else { break; } + } - const rclcpp::Duration time_diff = curr_time - prev_time; - const double time_diff_sec = time_diff.seconds(); - if (time_diff_sec < 0.0) { - continue; - } - - // quat to rpy - auto [ang_x, ang_y, ang_z] = quat_to_rpy(pose.orientation); - - // rpy update - ang_x += twist.angular.x * time_diff_sec; - ang_y += twist.angular.y * time_diff_sec; - ang_z += twist.angular.z * time_diff_sec; - tf2::Quaternion quat; - quat.setRPY(ang_x, ang_y, ang_z); + // dead reckoning from prev_odometry_ to latest_odometry_ + PoseStamped::SharedPtr prev_pose = std::make_shared(); + prev_pose->header = prev_odometry_->header; + prev_pose->pose = prev_odometry_->pose.pose; - // Convert twist to world frame - tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); - linear_velocity = tf2::quatRotate(quat, linear_velocity); - - // update - pose.position.x += linear_velocity.x() * time_diff_sec; - pose.position.y += linear_velocity.y() * time_diff_sec; - pose.position.z += linear_velocity.z() * time_diff_sec; - pose.orientation.x = quat.x(); - pose.orientation.y = quat.y(); - pose.orientation.z = quat.z(); - pose.orientation.w = quat.w(); - prev_time = curr_time; - } + Pose::SharedPtr DR_pose = std::make_shared(); + dead_reckon(prev_pose, latest_odometry_time, twist_buffer_, DR_pose); - // compare pose and latest_odometry_ + // compare dead reckoning pose and latest_odometry_ const Pose latest_ekf_pose = latest_odometry_->pose.pose; - const Pose ekf_to_odom = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose); - const geometry_msgs::msg::Point pos = ekf_to_odom.position; - const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.orientation); + const Pose ekf_to_DR = tier4_autoware_utils::inverseTransformPose(*DR_pose, latest_ekf_pose); + const geometry_msgs::msg::Point pos = ekf_to_DR.position; + const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_DR.orientation); const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; - const rclcpp::Time stamp = latest_odometry_->header.stamp; - // publish diff_pose for debug PoseStamped diff_pose; - diff_pose.header = latest_odometry_->header; - diff_pose.pose = ekf_to_odom; + diff_pose.header.stamp = latest_odometry_time; + diff_pose.header.frame_id = "base_link"; + diff_pose.pose = ekf_to_DR; diff_pose_pub_->publish(diff_pose); - const std::vector thresholds = {threshold_diff_position_x_, threshold_diff_position_y_, - threshold_diff_position_z_, threshold_diff_angle_x_, - threshold_diff_angle_y_, threshold_diff_angle_z_}; + // publish diagnostics + ThresholdValues threshold_values = + calculate_threshold((latest_odometry_time - prev_odometry_time).seconds()); + + const std::vector thresholds = {threshold_values.position_x, threshold_values.position_y, + threshold_values.position_z, threshold_values.angle_x, + threshold_values.angle_y, threshold_values.angle_z}; const std::vector labels = {"diff_position_x", "diff_position_y", "diff_position_z", "diff_angle_x", "diff_angle_y", "diff_angle_z"}; - // publish diagnostics DiagnosticStatus status; status.name = "localization: pose_instability_detector"; status.hardware_id = this->get_name(); @@ -166,13 +169,239 @@ void PoseInstabilityDetector::callback_timer() status.message = (all_ok ? "OK" : "WARN"); DiagnosticArray diagnostics; - diagnostics.header.stamp = stamp; + diagnostics.header.stamp = latest_odometry_time; diagnostics.status.emplace_back(status); diagnostics_pub_->publish(diagnostics); // prepare for next loop prev_odometry_ = latest_odometry_; - twist_buffer_.clear(); +} + +PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold( + double interval_sec) +{ + // Calculate maximum longitudinal difference + const double longitudinal_difference = + heading_velocity_maximum_ * heading_velocity_scale_factor_tolerance_ * 0.01 * interval_sec; + + // Calculate maximum lateral and vertical difference + double lateral_difference = 0.0; + + const std::vector heading_velocity_signs = {1.0, -1.0, -1.0, 1.0}; + const std::vector angular_velocity_signs = {1.0, 1.0, -1.0, -1.0}; + + const double nominal_variation_x = heading_velocity_maximum_ / angular_velocity_maximum_ * + sin(angular_velocity_maximum_ * interval_sec); + const double nominal_variation_y = heading_velocity_maximum_ / angular_velocity_maximum_ * + (1 - cos(angular_velocity_maximum_ * interval_sec)); + + for (int i = 0; i < 4; i++) { + const double edge_heading_velocity = + heading_velocity_maximum_ * + (1 + heading_velocity_signs[i] * heading_velocity_scale_factor_tolerance_ * 0.01); + const double edge_angular_velocity = + angular_velocity_maximum_ * + (1 + angular_velocity_signs[i] * angular_velocity_scale_factor_tolerance_ * 0.01) + + angular_velocity_signs[i] * angular_velocity_bias_tolerance_; + + const double edge_variation_x = + edge_heading_velocity / edge_angular_velocity * sin(edge_angular_velocity * interval_sec); + const double edge_variation_y = edge_heading_velocity / edge_angular_velocity * + (1 - cos(edge_angular_velocity * interval_sec)); + + const double diff_variation_x = edge_variation_x - nominal_variation_x; + const double diff_variation_y = edge_variation_y - nominal_variation_y; + + const double lateral_difference_candidate = abs( + diff_variation_x * sin(angular_velocity_maximum_ * interval_sec) - + diff_variation_y * cos(angular_velocity_maximum_ * interval_sec)); + lateral_difference = std::max(lateral_difference, lateral_difference_candidate); + } + + const double vertical_difference = lateral_difference; + + // Calculate maximum angular difference + const double roll_difference = + (angular_velocity_maximum_ * angular_velocity_scale_factor_tolerance_ * 0.01 + + angular_velocity_bias_tolerance_) * + interval_sec; + const double pitch_difference = roll_difference; + const double yaw_difference = roll_difference; + + // Set thresholds + ThresholdValues result_values; + result_values.position_x = longitudinal_difference + pose_estimator_longitudinal_tolerance_; + result_values.position_y = lateral_difference + pose_estimator_lateral_tolerance_; + result_values.position_z = vertical_difference + pose_estimator_vertical_tolerance_; + result_values.angle_x = roll_difference + pose_estimator_angular_tolerance_; + result_values.angle_y = pitch_difference + pose_estimator_angular_tolerance_; + result_values.angle_z = yaw_difference + pose_estimator_angular_tolerance_; + + return result_values; +} + +void PoseInstabilityDetector::dead_reckon( + PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, + const std::deque & twist_deque, Pose::SharedPtr & estimated_pose) +{ + // get start time + rclcpp::Time start_time = rclcpp::Time(initial_pose->header.stamp); + + // initialize estimated_pose + estimated_pose->position = initial_pose->pose.position; + estimated_pose->orientation = initial_pose->pose.orientation; + + // cut out necessary twist data + std::deque sliced_twist_deque = + clip_out_necessary_twist(twist_deque, start_time, end_time); + + // dead reckoning + rclcpp::Time prev_odometry_time = rclcpp::Time(sliced_twist_deque.front().header.stamp); + tf2::Quaternion prev_orientation; + tf2::fromMsg(estimated_pose->orientation, prev_orientation); + + for (size_t i = 1; i < sliced_twist_deque.size(); ++i) { + const rclcpp::Time curr_time = rclcpp::Time(sliced_twist_deque[i].header.stamp); + const double time_diff_sec = (curr_time - prev_odometry_time).seconds(); + + const Twist twist = sliced_twist_deque[i - 1].twist.twist; + + // variation of orientation (rpy update) + tf2::Quaternion delta_orientation; + tf2::Vector3 rotation_axis(twist.angular.x, twist.angular.y, twist.angular.z); + if (rotation_axis.length() > 0.0) { + delta_orientation.setRotation( + rotation_axis.normalized(), rotation_axis.length() * time_diff_sec); + } else { + delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); + } + + tf2::Quaternion curr_orientation; + curr_orientation = prev_orientation * delta_orientation; + curr_orientation.normalize(); + + // average quaternion of two frames + tf2::Quaternion average_quat = prev_orientation.slerp(curr_orientation, 0.5); + + // Convert twist to world frame (take average of two frames) + tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); + linear_velocity = tf2::quatRotate(average_quat, linear_velocity); + + // xyz update + estimated_pose->position.x += linear_velocity.x() * time_diff_sec; + estimated_pose->position.y += linear_velocity.y() * time_diff_sec; + estimated_pose->position.z += linear_velocity.z() * time_diff_sec; + + // update previous variables + prev_odometry_time = curr_time; + prev_orientation = curr_orientation; + } + estimated_pose->orientation.x = prev_orientation.x(); + estimated_pose->orientation.y = prev_orientation.y(); + estimated_pose->orientation.z = prev_orientation.z(); + estimated_pose->orientation.w = prev_orientation.w(); +} + +std::deque +PoseInstabilityDetector::clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time) +{ + // If there is only one element in the twist_buffer, return a deque that has the same twist + // from the start till the end + if (twist_buffer.size() == 1) { + TwistWithCovarianceStamped twist = twist_buffer.front(); + std::deque simple_twist_deque; + + twist.header.stamp = start_time; + simple_twist_deque.push_back(twist); + + twist.header.stamp = end_time; + simple_twist_deque.push_back(twist); + + return simple_twist_deque; + } + + // get iterator to the element that is right before start_time (if it does not exist, start_it = + // twist_buffer.begin()) + auto start_it = twist_buffer.begin(); + + for (auto it = twist_buffer.begin(); it != twist_buffer.end(); ++it) { + if (rclcpp::Time(it->header.stamp) > start_time) { + break; + } + start_it = it; + } + + // get iterator to the element that is right after end_time (if it does not exist, end_it = + // twist_buffer.end()) + auto end_it = twist_buffer.end(); + end_it--; + for (auto it = end_it; it != twist_buffer.begin(); --it) { + if (rclcpp::Time(it->header.stamp) < end_time) { + break; + } + end_it = it; + } + + // Create result deque + std::deque result_deque(start_it, end_it); + + // If the first element is later than start_time, add the first element to the front of the + // result_deque + if (rclcpp::Time(result_deque.front().header.stamp) > start_time) { + TwistWithCovarianceStamped start_twist = *start_it; + start_twist.header.stamp = start_time; + result_deque.push_front(start_twist); + } else { + // If the first element is earlier than start_time, interpolate the first element + rclcpp::Time time0 = rclcpp::Time(result_deque[0].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[1].header.stamp); + double ratio = (start_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[0].twist.twist; + Twist twist1 = result_deque[1].twist.twist; + result_deque[0].twist.twist.linear.x = twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[0].twist.twist.linear.y = twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[0].twist.twist.linear.z = twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[0].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[0].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[0].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[0].header.stamp = start_time; + } + + // If the last element is earlier than end_time, add the last element to the back of the + // result_deque + if (rclcpp::Time(result_deque.back().header.stamp) < end_time) { + TwistWithCovarianceStamped end_twist = *end_it; + end_twist.header.stamp = end_time; + result_deque.push_back(end_twist); + } else { + // If the last element is later than end_time, interpolate the last element + rclcpp::Time time0 = rclcpp::Time(result_deque[result_deque.size() - 2].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[result_deque.size() - 1].header.stamp); + double ratio = (end_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[result_deque.size() - 2].twist.twist; + Twist twist1 = result_deque[result_deque.size() - 1].twist.twist; + result_deque[result_deque.size() - 1].twist.twist.linear.x = + twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.y = + twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.z = + twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[result_deque.size() - 1].header.stamp = end_time; + } + return result_deque; } #include diff --git a/localization/pose_instability_detector/src/pose_instability_detector.hpp b/localization/pose_instability_detector/src/pose_instability_detector.hpp deleted file mode 100644 index 761a10b7a6bf7..0000000000000 --- a/localization/pose_instability_detector/src/pose_instability_detector.hpp +++ /dev/null @@ -1,70 +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. - -#ifndef POSE_INSTABILITY_DETECTOR_HPP_ -#define POSE_INSTABILITY_DETECTOR_HPP_ - -#include - -#include -#include -#include -#include - -#include - -class PoseInstabilityDetector : public rclcpp::Node -{ - using Quaternion = geometry_msgs::msg::Quaternion; - using Twist = geometry_msgs::msg::Twist; - using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; - using Pose = geometry_msgs::msg::Pose; - using PoseStamped = geometry_msgs::msg::PoseStamped; - using Odometry = nav_msgs::msg::Odometry; - using KeyValue = diagnostic_msgs::msg::KeyValue; - using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; - using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; - -public: - explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - -private: - void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr); - void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); - void callback_timer(); - - // subscribers and timer - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr twist_sub_; - rclcpp::TimerBase::SharedPtr timer_; - - // publisher - rclcpp::Publisher::SharedPtr diff_pose_pub_; - rclcpp::Publisher::SharedPtr diagnostics_pub_; - - // parameters - const double threshold_diff_position_x_; - const double threshold_diff_position_y_; - const double threshold_diff_position_z_; - const double threshold_diff_angle_x_; - const double threshold_diff_angle_y_; - const double threshold_diff_angle_z_; - - // variables - std::optional latest_odometry_ = std::nullopt; - std::optional prev_odometry_ = std::nullopt; - std::vector twist_buffer_; -}; - -#endif // POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp index 5ea03859d7731..9300984967d4b 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/localization/pose_instability_detector/test/test.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "../src/pose_instability_detector.hpp" +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" #include "test_message_helper_node.hpp" #include @@ -81,6 +81,11 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N timestamp.nanosec = 0; helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + // send the twist message1 (move 1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + // process the above message (by timer_callback) helper_->received_diagnostic_array_flag = false; while (!helper_->received_diagnostic_array_flag) { @@ -88,11 +93,6 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // send the twist message1 (move 1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 5e8; - helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); - // send the twist message2 (move 1m in x direction) timestamp.sec = 1; timestamp.nanosec = 0; @@ -101,7 +101,9 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N // send the second odometry message (finish x = 12) timestamp.sec = 2; timestamp.nanosec = 0; - helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); // process the above messages (by timer_callback) helper_->received_diagnostic_array_flag = false; @@ -124,6 +126,11 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL timestamp.nanosec = 0; helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + // send the twist message1 (move 0.1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + // process the above message (by timer_callback) helper_->received_diagnostic_array_flag = false; while (!helper_->received_diagnostic_array_flag) { @@ -131,11 +138,6 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // send the twist message1 (move 0.1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 5e8; - helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); - // send the twist message2 (move 0.1m in x direction) timestamp.sec = 1; timestamp.nanosec = 0; @@ -144,7 +146,9 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL // send the second odometry message (finish x = 12) timestamp.sec = 2; timestamp.nanosec = 0; - helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); // process the above messages (by timer_callback) helper_->received_diagnostic_array_flag = false; diff --git a/localization/stop_filter/include/stop_filter/stop_filter.hpp b/localization/stop_filter/include/stop_filter/stop_filter.hpp index 24145a7920d91..e8428788820b2 100644 --- a/localization/stop_filter/include/stop_filter/stop_filter.hpp +++ b/localization/stop_filter/include/stop_filter/stop_filter.hpp @@ -52,6 +52,6 @@ class StopFilter : public rclcpp::Node /** * @brief set odometry measurement */ - void callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); + void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg); }; #endif // STOP_FILTER__STOP_FILTER_HPP_ diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index 4d6b2c6240867..c1c4de2fb6b6e 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -31,13 +31,13 @@ StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) wz_threshold_ = declare_parameter("wz_threshold"); sub_odom_ = create_subscription( - "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1)); + "input/odom", 1, std::bind(&StopFilter::callback_odometry, this, _1)); pub_odom_ = create_publisher("output/odom", 1); pub_stop_flag_ = create_publisher("debug/stop_flag", 1); } -void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +void StopFilter::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) { tier4_debug_msgs::msg::BoolStamped stop_flag_msg; stop_flag_msg.stamp = msg->header.stamp; diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index de675da5cc2d4..d4c8b71fa6b66 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -29,6 +29,7 @@ sophus std_msgs tf2_ros + tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index d0dfc87067f7f..1126d6260b911 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -15,7 +15,7 @@ #include "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" #include -#include +#include #include #include @@ -198,7 +198,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_sign_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0.6f, 0.6f, 0.6f, 0.999f); + marker.color = tier4_autoware_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); marker.scale.x = 0.1; marker.ns = ns; marker.id = id++; @@ -228,7 +228,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_polygon_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0.4f, 0.4f, 0.8f, 0.999f); + marker.color = tier4_autoware_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); marker.scale.x = 0.2; marker.ns = ns; marker.id = id++; diff --git a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp index 90cd62883e339..e6e59a836f6db 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp @@ -93,7 +93,8 @@ class ParticleVisualize : public rclcpp::Node marker.scale.y = 0.1; marker.scale.z = 0.1; - marker.color = common::color_scale::rainbow(bound_weight(p.weight)); + marker.color = + static_cast(common::color_scale::rainbow(bound_weight(p.weight))); marker.pose.orientation = p.pose.orientation; marker.pose.position.x = p.pose.position.x; marker.pose.position.y = p.pose.position.y; diff --git a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp index 955ed84fdea2d..888baf0bea0ac 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp @@ -45,7 +45,8 @@ void ParticleVisualizer::publish(const ParticleArray & msg) marker.scale.x = 0.3; marker.scale.y = 0.1; marker.scale.z = 0.1; - marker.color = common::color_scale::rainbow(boundWeight(p.weight)); + marker.color = + static_cast(common::color_scale::rainbow(boundWeight(p.weight))); marker.pose.orientation = p.pose.orientation; marker.pose.position.x = p.pose.position.x; marker.pose.position.y = p.pose.position.y; diff --git a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp index b321986240b69..e8c8890561a7f 100644 --- a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp @@ -136,7 +136,7 @@ void GnssParticleCorrector::publish_marker(const Eigen::Vector3f & position, boo marker.pose.position.z = latest_height_.data; float prob = i / 4.0f; - marker.color = common::color_scale::rainbow(prob); + marker.color = static_cast(common::color_scale::rainbow(prob)); marker.color.a = 0.5f; marker.scale.x = 0.1; drawCircle(marker.points, weight_manager_.inverse_normal_pdf(prob, is_rtk_fixed)); diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index ebd9c424edf5e..62d8ed826b373 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -169,7 +169,7 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const marker.header.frame_id = "map"; marker.id = id++; marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0, 0, 1.0f, 1.0f); + marker.color = tier4_autoware_utils::createMarkerColor(0, 0, 1.0f, 1.0f); marker.scale.x = 0.1; Eigen::Vector2f xy = area.real_scale(); marker.points.push_back(point_msg(xy.x(), xy.y())); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp index c6c34f0e28a42..8a5fc65e2fac3 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp @@ -42,7 +42,8 @@ void MarkerModule::publish_marker( marker.type = Marker::ARROW; marker.id = i; marker.ns = "arrow"; - marker.color = common::color_scale::rainbow(normalize(scores.at(i))); + marker.color = + static_cast(common::color_scale::rainbow(normalize(scores.at(i)))); marker.color.a = 0.5; marker.pose.position.x = position.x(); diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 4eaf8600dcab4..a6f4ac7f81275 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -4,7 +4,6 @@ map_loader 0.1.0 The map_loader package - Ryohsuke Mitsudome Yamato Ando Ryu Yamamoto Koji Minoda @@ -15,6 +14,8 @@ Shintaro Sakoda Apache License 2.0 + Ryohsuke Mitsudome + Koji Minoda ament_cmake_auto autoware_cmake diff --git a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp b/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp index 345703a19bc06..cda3bc5c49a0c 100644 --- a/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp +++ b/map/map_tf_generator/include/map_tf_generator/uniform_random.hpp @@ -18,7 +18,7 @@ #include #include -std::vector UniformRandom(const size_t max_exclusive, const size_t n) +std::vector inline uniform_random(const size_t max_exclusive, const size_t n) { std::default_random_engine generator; std::uniform_int_distribution distribution(0, max_exclusive - 1); diff --git a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp b/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp index 69a66cf52c865..d4b8ccb797224 100644 --- a/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/pcd_map_tf_generator_node.cpp @@ -26,22 +26,22 @@ #include #include +#include -constexpr size_t N_SAMPLES = 20; +constexpr size_t n_samples = 20; class PcdMapTFGeneratorNode : public rclcpp::Node { public: using PointCloud = pcl::PointCloud; explicit PcdMapTFGeneratorNode(const rclcpp::NodeOptions & options) - : Node("pcd_map_tf_generator", options) + : Node("pcd_map_tf_generator", options), + map_frame_(declare_parameter("map_frame")), + viewer_frame_(declare_parameter("viewer_frame")) { - map_frame_ = declare_parameter("map_frame"); - viewer_frame_ = declare_parameter("viewer_frame"); - sub_ = create_subscription( "pointcloud_map", rclcpp::QoS{1}.transient_local(), - std::bind(&PcdMapTFGeneratorNode::onPointCloud, this, std::placeholders::_1)); + std::bind(&PcdMapTFGeneratorNode::on_point_cloud, this, std::placeholders::_1)); static_broadcaster_ = std::make_shared(this); } @@ -53,7 +53,7 @@ class PcdMapTFGeneratorNode : public rclcpp::Node std::shared_ptr static_broadcaster_; - void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr clouds_ros) + void on_point_cloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr clouds_ros) { // fix random seed to produce the same viewer position every time // 3939 is just the author's favorite number @@ -62,32 +62,32 @@ class PcdMapTFGeneratorNode : public rclcpp::Node PointCloud clouds; pcl::fromROSMsg(*clouds_ros, clouds); - const std::vector indices = UniformRandom(clouds.size(), N_SAMPLES); + const std::vector indices = uniform_random(clouds.size(), n_samples); double coordinate[3] = {0, 0, 0}; for (const auto i : indices) { coordinate[0] += clouds.points[i].x; coordinate[1] += clouds.points[i].y; coordinate[2] += clouds.points[i].z; } - coordinate[0] = coordinate[0] / indices.size(); - coordinate[1] = coordinate[1] / indices.size(); - coordinate[2] = coordinate[2] / indices.size(); - - geometry_msgs::msg::TransformStamped static_transformStamped; - static_transformStamped.header.stamp = this->now(); - static_transformStamped.header.frame_id = map_frame_; - static_transformStamped.child_frame_id = viewer_frame_; - static_transformStamped.transform.translation.x = coordinate[0]; - static_transformStamped.transform.translation.y = coordinate[1]; - static_transformStamped.transform.translation.z = coordinate[2]; + coordinate[0] = coordinate[0] / static_cast(indices.size()); + coordinate[1] = coordinate[1] / static_cast(indices.size()); + coordinate[2] = coordinate[2] / static_cast(indices.size()); + + geometry_msgs::msg::TransformStamped static_transform_stamped; + static_transform_stamped.header.stamp = this->now(); + static_transform_stamped.header.frame_id = map_frame_; + static_transform_stamped.child_frame_id = viewer_frame_; + static_transform_stamped.transform.translation.x = coordinate[0]; + static_transform_stamped.transform.translation.y = coordinate[1]; + static_transform_stamped.transform.translation.z = coordinate[2]; tf2::Quaternion quat; quat.setRPY(0, 0, 0); - static_transformStamped.transform.rotation.x = quat.x(); - static_transformStamped.transform.rotation.y = quat.y(); - static_transformStamped.transform.rotation.z = quat.z(); - static_transformStamped.transform.rotation.w = quat.w(); + static_transform_stamped.transform.rotation.x = quat.x(); + static_transform_stamped.transform.rotation.y = quat.y(); + static_transform_stamped.transform.rotation.z = quat.z(); + static_transform_stamped.transform.rotation.w = quat.w(); - static_broadcaster_->sendTransform(static_transformStamped); + static_broadcaster_->sendTransform(static_transform_stamped); RCLCPP_INFO_STREAM( get_logger(), "broadcast static tf. map_frame:" diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp index f242254a456a1..df3075bdce1fe 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -29,14 +29,13 @@ class VectorMapTFGeneratorNode : public rclcpp::Node { public: explicit VectorMapTFGeneratorNode(const rclcpp::NodeOptions & options) - : Node("vector_map_tf_generator", options) + : Node("vector_map_tf_generator", options), + map_frame_(declare_parameter("map_frame")), + viewer_frame_(declare_parameter("viewer_frame")) { - map_frame_ = declare_parameter("map_frame"); - viewer_frame_ = declare_parameter("viewer_frame"); - sub_ = create_subscription( "vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&VectorMapTFGeneratorNode::onVectorMap, this, std::placeholders::_1)); + std::bind(&VectorMapTFGeneratorNode::on_vector_map, this, std::placeholders::_1)); static_broadcaster_ = std::make_shared(this); } @@ -49,7 +48,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node std::shared_ptr static_broadcaster_; std::shared_ptr lanelet_map_ptr_; - void onVectorMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) + void on_vector_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); @@ -66,27 +65,27 @@ class VectorMapTFGeneratorNode : public rclcpp::Node points_z.push_back(point_z); } const double coordinate_x = - std::accumulate(points_x.begin(), points_x.end(), 0.0) / points_x.size(); + std::accumulate(points_x.begin(), points_x.end(), 0.0) / static_cast(points_x.size()); const double coordinate_y = - std::accumulate(points_y.begin(), points_y.end(), 0.0) / points_y.size(); + std::accumulate(points_y.begin(), points_y.end(), 0.0) / static_cast(points_y.size()); const double coordinate_z = - std::accumulate(points_z.begin(), points_z.end(), 0.0) / points_z.size(); + std::accumulate(points_z.begin(), points_z.end(), 0.0) / static_cast(points_z.size()); - geometry_msgs::msg::TransformStamped static_transformStamped; - static_transformStamped.header.stamp = this->now(); - static_transformStamped.header.frame_id = map_frame_; - static_transformStamped.child_frame_id = viewer_frame_; - static_transformStamped.transform.translation.x = coordinate_x; - static_transformStamped.transform.translation.y = coordinate_y; - static_transformStamped.transform.translation.z = coordinate_z; + geometry_msgs::msg::TransformStamped static_transform_stamped; + static_transform_stamped.header.stamp = this->now(); + static_transform_stamped.header.frame_id = map_frame_; + static_transform_stamped.child_frame_id = viewer_frame_; + static_transform_stamped.transform.translation.x = coordinate_x; + static_transform_stamped.transform.translation.y = coordinate_y; + static_transform_stamped.transform.translation.z = coordinate_z; tf2::Quaternion quat; quat.setRPY(0, 0, 0); - static_transformStamped.transform.rotation.x = quat.x(); - static_transformStamped.transform.rotation.y = quat.y(); - static_transformStamped.transform.rotation.z = quat.z(); - static_transformStamped.transform.rotation.w = quat.w(); + static_transform_stamped.transform.rotation.x = quat.x(); + static_transform_stamped.transform.rotation.y = quat.y(); + static_transform_stamped.transform.rotation.z = quat.z(); + static_transform_stamped.transform.rotation.w = quat.w(); - static_broadcaster_->sendTransform(static_transformStamped); + static_broadcaster_->sendTransform(static_transform_stamped); RCLCPP_INFO_STREAM( get_logger(), "broadcast static tf. map_frame:" diff --git a/map/map_tf_generator/test/test_uniform_random.cpp b/map/map_tf_generator/test/test_uniform_random.cpp index 455edc2d5dfd2..19e558ebb2add 100644 --- a/map/map_tf_generator/test/test_uniform_random.cpp +++ b/map/map_tf_generator/test/test_uniform_random.cpp @@ -21,10 +21,10 @@ using testing::Each; using testing::Ge; using testing::Lt; -TEST(UniformRandom, UniformRandom) +TEST(uniform_random, uniform_random) { { - const std::vector random = UniformRandom(4, 0); + const std::vector random = uniform_random(4, 0); ASSERT_EQ(random.size(), static_cast(0)); } @@ -35,7 +35,7 @@ TEST(UniformRandom, UniformRandom) const size_t max_exclusive = 4; for (int i = 0; i < 50; i++) { - const std::vector random = UniformRandom(4, 10); + const std::vector random = uniform_random(4, 10); ASSERT_EQ(random.size(), 10U); ASSERT_THAT(random, Each(AllOf(Ge(min_inclusive), Lt(max_exclusive)))); // in range [0, 4) } diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml index 421f04257de67..e84bd8a80c202 100644 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ b/map/util/lanelet2_map_preprocessor/package.xml @@ -4,7 +4,6 @@ lanelet2_map_preprocessor 0.1.0 The lanelet2_map_preprocessor package - Ryohsuke Mitsudome Yamato Ando Kento Yabuuchi Masahiro Sakamoto diff --git a/perception/autoware_crosswalk_traffic_light_estimator/CMakeLists.txt b/perception/autoware_crosswalk_traffic_light_estimator/CMakeLists.txt new file mode 100644 index 0000000000000..31a098a8686ec --- /dev/null +++ b/perception/autoware_crosswalk_traffic_light_estimator/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_crosswalk_traffic_light_estimator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +########### +## Build ## +########### + +include_directories() + +ament_auto_add_library(autoware_crosswalk_traffic_light_estimator SHARED + src/node.cpp +) + +rclcpp_components_register_node(autoware_crosswalk_traffic_light_estimator + PLUGIN "autoware::crosswalk_traffic_light_estimator::CrosswalkTrafficLightEstimatorNode" + EXECUTABLE crosswalk_traffic_light_estimator_node +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +############# +## Install ## +############# + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/perception/crosswalk_traffic_light_estimator/README.md b/perception/autoware_crosswalk_traffic_light_estimator/README.md similarity index 100% rename from perception/crosswalk_traffic_light_estimator/README.md rename to perception/autoware_crosswalk_traffic_light_estimator/README.md diff --git a/perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml b/perception/autoware_crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml similarity index 100% rename from perception/crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml rename to perception/autoware_crosswalk_traffic_light_estimator/config/crosswalk_traffic_light_estimator.param.yaml diff --git a/perception/crosswalk_traffic_light_estimator/images/intersection1.svg b/perception/autoware_crosswalk_traffic_light_estimator/images/intersection1.svg similarity index 100% rename from perception/crosswalk_traffic_light_estimator/images/intersection1.svg rename to perception/autoware_crosswalk_traffic_light_estimator/images/intersection1.svg diff --git a/perception/crosswalk_traffic_light_estimator/images/intersection2.svg b/perception/autoware_crosswalk_traffic_light_estimator/images/intersection2.svg similarity index 100% rename from perception/crosswalk_traffic_light_estimator/images/intersection2.svg rename to perception/autoware_crosswalk_traffic_light_estimator/images/intersection2.svg diff --git a/perception/crosswalk_traffic_light_estimator/images/straight.drawio.svg b/perception/autoware_crosswalk_traffic_light_estimator/images/straight.drawio.svg similarity index 100% rename from perception/crosswalk_traffic_light_estimator/images/straight.drawio.svg rename to perception/autoware_crosswalk_traffic_light_estimator/images/straight.drawio.svg diff --git a/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp new file mode 100644 index 0000000000000..de53d78d7f782 --- /dev/null +++ b/perception/autoware_crosswalk_traffic_light_estimator/include/autoware_crosswalk_traffic_light_estimator/node.hpp @@ -0,0 +1,122 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ +#define AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +namespace autoware::crosswalk_traffic_light_estimator +{ + +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_planning_msgs::msg::LaneletRoute; +using tier4_autoware_utils::DebugPublisher; +using tier4_autoware_utils::StopWatch; +using tier4_debug_msgs::msg::Float64Stamped; +using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; +using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; +using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; +using TrafficSignalAndTime = std::pair; +using TrafficLightIdMap = std::unordered_map; + +using TrafficLightIdArray = std::unordered_map>; + +class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node +{ +public: + explicit CrosswalkTrafficLightEstimatorNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Subscription::SharedPtr sub_traffic_light_array_; + rclcpp::Publisher::SharedPtr pub_traffic_light_array_; + + lanelet::LaneletMapPtr lanelet_map_ptr_; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; + lanelet::routing::RoutingGraphPtr routing_graph_ptr_; + std::shared_ptr overall_graphs_ptr_; + + lanelet::ConstLanelets conflicting_crosswalks_; + + void onMap(const LaneletMapBin::ConstSharedPtr msg); + void onRoute(const LaneletRoute::ConstSharedPtr msg); + void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg); + + void updateLastDetectedSignal(const TrafficLightIdMap & traffic_signals); + void updateLastDetectedSignals(const TrafficLightIdMap & traffic_signals); + void updateFlashingState(const TrafficSignal & signal); + uint8_t updateAndGetColorState(const TrafficSignal & signal); + void setCrosswalkTrafficSignal( + const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, + TrafficSignalArray & output); + + lanelet::ConstLanelets getNonRedLanelets( + const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const; + + uint8_t estimateCrosswalkTrafficSignal( + const lanelet::ConstLanelet & crosswalk, const lanelet::ConstLanelets & non_red_lanelets) const; + + boost::optional getHighestConfidenceTrafficSignal( + 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; + + void removeDuplicateIds(TrafficSignalArray & signal_array) const; + + // Node param + bool use_last_detect_color_; + double last_detect_color_hold_time_; + double last_colors_hold_time_; + + // Signal history + TrafficLightIdMap last_detect_color_; + TrafficLightIdArray last_colors_; + + // State + std::map is_flashing_; + std::map current_color_state_; + + // Stop watch + StopWatch stop_watch_; + + // Debug + std::shared_ptr pub_processing_time_; +}; + +} // namespace autoware::crosswalk_traffic_light_estimator + +#endif // AUTOWARE_CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ diff --git a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml b/perception/autoware_crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml similarity index 78% rename from perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml rename to perception/autoware_crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml index 21d718c3439cd..2e1437ecd7d93 100644 --- a/perception/crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml +++ b/perception/autoware_crosswalk_traffic_light_estimator/launch/crosswalk_traffic_light_estimator.launch.xml @@ -11,9 +11,9 @@ limitations under the License. --> - + - + diff --git a/perception/autoware_crosswalk_traffic_light_estimator/package.xml b/perception/autoware_crosswalk_traffic_light_estimator/package.xml new file mode 100644 index 0000000000000..d844ea27b52b5 --- /dev/null +++ b/perception/autoware_crosswalk_traffic_light_estimator/package.xml @@ -0,0 +1,30 @@ + + + autoware_crosswalk_traffic_light_estimator + 0.1.0 + The autoware_crosswalk_traffic_light_estimator package + + Satoshi Ota + Shunsuke Miura + Tao Zhong + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_map_msgs + autoware_perception_msgs + autoware_planning_msgs + lanelet2_extension + rclcpp + rclcpp_components + tier4_autoware_utils + tier4_perception_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json b/perception/autoware_crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json similarity index 100% rename from perception/crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json rename to perception/autoware_crosswalk_traffic_light_estimator/schema/crosswalk_traffic_light_estimator.schema.json diff --git a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp new file mode 100644 index 0000000000000..2c4da0b95b161 --- /dev/null +++ b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp @@ -0,0 +1,546 @@ +// 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. +// 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 "autoware_crosswalk_traffic_light_estimator/node.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace autoware::crosswalk_traffic_light_estimator +{ +namespace +{ + +bool hasMergeLane( + const lanelet::ConstLanelet & lanelet_1, const lanelet::ConstLanelet & lanelet_2, + const lanelet::routing::RoutingGraphPtr & routing_graph_ptr) +{ + const auto next_lanelets_1 = routing_graph_ptr->following(lanelet_1); + const auto next_lanelets_2 = routing_graph_ptr->following(lanelet_2); + + for (const auto & next_lanelet_1 : next_lanelets_1) { + for (const auto & next_lanelet_2 : next_lanelets_2) { + if (next_lanelet_1.id() == next_lanelet_2.id()) { + return true; + } + } + } + + return false; +} + +bool hasMergeLane( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr & routing_graph_ptr) +{ + for (size_t i = 0; i < lanelets.size(); ++i) { + for (size_t j = i + 1; j < lanelets.size(); ++j) { + const auto lanelet_1 = lanelets.at(i); + const auto lanelet_2 = lanelets.at(j); + + if (lanelet_1.id() == lanelet_2.id()) { + continue; + } + + const std::string turn_direction_1 = lanelet_1.attributeOr("turn_direction", "none"); + const std::string turn_direction_2 = lanelet_2.attributeOr("turn_direction", "none"); + if (turn_direction_1 == turn_direction_2) { + continue; + } + + if (!hasMergeLane(lanelet_1, lanelet_2, routing_graph_ptr)) { + continue; + } + + return true; + } + } + + return false; +} +} // namespace + +CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( + const rclcpp::NodeOptions & options) +: Node("crosswalk_traffic_light_estimator", options) +{ + using std::placeholders::_1; + + use_last_detect_color_ = declare_parameter("use_last_detect_color"); + last_detect_color_hold_time_ = declare_parameter("last_detect_color_hold_time"); + last_colors_hold_time_ = declare_parameter("last_colors_hold_time"); + + sub_map_ = create_subscription( + "~/input/vector_map", rclcpp::QoS{1}.transient_local(), + std::bind(&CrosswalkTrafficLightEstimatorNode::onMap, this, _1)); + sub_route_ = create_subscription( + "~/input/route", rclcpp::QoS{1}.transient_local(), + std::bind(&CrosswalkTrafficLightEstimatorNode::onRoute, this, _1)); + sub_traffic_light_array_ = create_subscription( + "~/input/classified/traffic_signals", rclcpp::QoS{1}, + std::bind(&CrosswalkTrafficLightEstimatorNode::onTrafficLightArray, this, _1)); + + pub_traffic_light_array_ = + this->create_publisher("~/output/traffic_signals", rclcpp::QoS{1}); + pub_processing_time_ = std::make_shared(this, "~/debug"); +} + +void CrosswalkTrafficLightEstimatorNode::onMap(const LaneletMapBin::ConstSharedPtr msg) +{ + RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Start loading lanelet"); + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Pedestrian); + lanelet::routing::RoutingGraphConstPtr vehicle_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules); + lanelet::routing::RoutingGraphConstPtr pedestrian_graph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules); + lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); + overall_graphs_ptr_ = + std::make_shared(overall_graphs); + RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded"); +} + +void CrosswalkTrafficLightEstimatorNode::onRoute(const LaneletRoute::ConstSharedPtr msg) +{ + if (lanelet_map_ptr_ == nullptr) { + RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map"); + return; + } + + lanelet::ConstLanelets route_lanelets; + for (const auto & segment : msg->segments) { + for (const auto & primitive : segment.primitives) { + try { + route_lanelets.push_back(lanelet_map_ptr_->laneletLayer.get(primitive.id)); + } catch (const lanelet::NoSuchPrimitiveError & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + return; + } + } + } + + conflicting_crosswalks_.clear(); + + for (const auto & route_lanelet : route_lanelets) { + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflict_lls = + overall_graphs_ptr_->conflictingInGraph(route_lanelet, PEDESTRIAN_GRAPH_ID); + for (const auto & lanelet : conflict_lls) { + conflicting_crosswalks_.push_back(lanelet); + } + } +} + +void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray( + const TrafficSignalArray::ConstSharedPtr msg) +{ + StopWatch stop_watch; + stop_watch.tic("Total"); + + TrafficSignalArray output = *msg; + + TrafficLightIdMap traffic_light_id_map; + for (const auto & traffic_signal : msg->traffic_light_groups) { + traffic_light_id_map[traffic_signal.traffic_light_group_id] = + std::pair(traffic_signal, get_clock()->now()); + } + for (const auto & crosswalk : conflicting_crosswalks_) { + constexpr int VEHICLE_GRAPH_ID = 0; + const auto conflict_lls = overall_graphs_ptr_->conflictingInGraph(crosswalk, VEHICLE_GRAPH_ID); + const auto non_red_lanelets = getNonRedLanelets(conflict_lls, traffic_light_id_map); + + const auto crosswalk_tl_color = estimateCrosswalkTrafficSignal(crosswalk, non_red_lanelets); + setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, *msg, output); + } + + removeDuplicateIds(output); + + updateLastDetectedSignal(traffic_light_id_map); + updateLastDetectedSignals(traffic_light_id_map); + + pub_traffic_light_array_->publish(output); + pub_processing_time_->publish("processing_time_ms", stop_watch.toc("Total")); + + return; +} + +void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( + const TrafficLightIdMap & traffic_light_id_map) +{ + for (const auto & input_traffic_signal : traffic_light_id_map) { + const auto & elements = input_traffic_signal.second.first.elements; + + if (elements.empty()) { + continue; + } + + if (elements.front().color == TrafficSignalElement::UNKNOWN) { + continue; + } + + const auto & id = input_traffic_signal.second.first.traffic_light_group_id; + + if (last_detect_color_.count(id) == 0) { + last_detect_color_.insert(std::make_pair(id, input_traffic_signal.second)); + continue; + } + + last_detect_color_.at(id) = input_traffic_signal.second; + } + + std::vector erase_id_list; + for (auto & last_traffic_signal : last_detect_color_) { + const auto & id = last_traffic_signal.second.first.traffic_light_group_id; + + if (traffic_light_id_map.count(id) == 0) { + // hold signal recognition results for [last_detect_color_hold_time_] seconds. + const auto time_from_last_detected = + (get_clock()->now() - last_traffic_signal.second.second).seconds(); + if (time_from_last_detected > last_detect_color_hold_time_) { + erase_id_list.emplace_back(id); + } + } + } + for (const auto id : erase_id_list) { + last_detect_color_.erase(id); + is_flashing_.erase(id); + current_color_state_.erase(id); + } +} + +void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignals( + const TrafficLightIdMap & traffic_light_id_map) +{ + for (const auto & input_traffic_signal : traffic_light_id_map) { + const auto & elements = input_traffic_signal.second.first.elements; + + if (elements.empty()) { + continue; + } + + if ( + elements.front().color == TrafficSignalElement::UNKNOWN && elements.front().confidence == 1) { + continue; + } + + const auto & id = input_traffic_signal.second.first.traffic_light_group_id; + + if (last_colors_.count(id) == 0) { + std::vector signal{input_traffic_signal.second}; + last_colors_.insert(std::make_pair(id, signal)); + continue; + } + + last_colors_.at(id).push_back(input_traffic_signal.second); + } + + std::vector erase_id_list; + for (auto & last_traffic_signal : last_colors_) { + const auto & id = last_traffic_signal.first; + for (auto it = last_traffic_signal.second.begin(); it != last_traffic_signal.second.end();) { + auto sig = (*it).first; + rclcpp::Time t = (*it).second; + + // hold signal recognition results for [last_colors_hold_time_] seconds. + const auto time_from_last_detected = (get_clock()->now() - t).seconds(); + if (time_from_last_detected > last_colors_hold_time_) { + it = last_traffic_signal.second.erase(it); + } else { + ++it; + } + } + if (last_traffic_signal.second.empty()) { + erase_id_list.emplace_back(id); + } + } + for (const auto id : erase_id_list) last_colors_.erase(id); +} + +void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( + const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, + TrafficSignalArray & output) +{ + const auto tl_reg_elems = crosswalk.regulatoryElementsAs(); + + std::unordered_map valid_id2idx_map; // detected traffic light + + for (size_t i = 0; i < msg.traffic_light_groups.size(); ++i) { + auto signal = msg.traffic_light_groups[i]; + valid_id2idx_map[signal.traffic_light_group_id] = i; + } + + for (const auto & tl_reg_elem : tl_reg_elems) { + auto id = tl_reg_elem->id(); + // if valid prediction exists, overwrite the estimation; else, use the estimation + if (valid_id2idx_map.count(id)) { + size_t idx = valid_id2idx_map[id]; + auto signal = msg.traffic_light_groups[idx]; + updateFlashingState(signal); // check if it is flashing + // update output msg according to flashing and current state + output.traffic_light_groups[idx].elements[0].color = updateAndGetColorState(signal); + } else { + 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_light_group_id = id; + output.traffic_light_groups.push_back(output_traffic_signal); + } + } +} + +void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal) +{ + const auto id = signal.traffic_light_group_id; + + // no record of detected color in last_detect_color_hold_time_ + if (is_flashing_.count(id) == 0) { + is_flashing_.insert(std::make_pair(id, false)); + return; + } + + // flashing green + if ( + signal.elements.front().color == TrafficSignalElement::UNKNOWN && + signal.elements.front().confidence != 0 && // not due to occlusion + current_color_state_.at(id) != TrafficSignalElement::UNKNOWN) { + is_flashing_.at(id) = true; + return; + } + + // history exists + if (last_colors_.count(id) > 0) { + std::vector history = last_colors_.at(id); + for (const auto & h : history) { + if (h.first.elements.front().color != signal.elements.front().color) { + // keep the current value if not same with input signal + return; + } + } + // all history is same with input signal + is_flashing_.at(id) = false; + } + + // no record of detected color in last_color_hold_time_ + // keep the current value + return; +} + +uint8_t CrosswalkTrafficLightEstimatorNode::updateAndGetColorState(const TrafficSignal & signal) +{ + const auto id = signal.traffic_light_group_id; + const auto color = signal.elements[0].color; + + if (current_color_state_.count(id) == 0) { + current_color_state_.insert(std::make_pair(id, color)); + } else if (is_flashing_.at(id) == false) { + current_color_state_.at(id) = color; + } else if (is_flashing_.at(id) == true) { + if ( + current_color_state_.at(id) == TrafficSignalElement::GREEN && + color == TrafficSignalElement::RED) { + current_color_state_.at(id) = TrafficSignalElement::RED; + } else if ( + current_color_state_.at(id) == TrafficSignalElement::RED && + color == TrafficSignalElement::GREEN) { + current_color_state_.at(id) = TrafficSignalElement::GREEN; + } else if (current_color_state_.at(id) == TrafficSignalElement::UNKNOWN) { + if (color == TrafficSignalElement::GREEN || color == TrafficSignalElement::UNKNOWN) + current_color_state_.at(id) = TrafficSignalElement::GREEN; + if (color == TrafficSignalElement::RED) + current_color_state_.at(id) = TrafficSignalElement::RED; + } + } + + return current_color_state_.at(id); +} + +lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets( + const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const +{ + lanelet::ConstLanelets non_red_lanelets{}; + + for (const auto & lanelet : lanelets) { + const auto tl_reg_elems = lanelet.regulatoryElementsAs(); + + if (tl_reg_elems.empty()) { + continue; + } + + const auto tl_reg_elem = tl_reg_elems.front(); + const auto current_detected_signal = + 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() == TrafficSignalElement::GREEN || + current_detected_signal.get() == TrafficSignalElement::AMBER + : true; + + const auto current_is_unknown_or_none = + current_detected_signal ? current_detected_signal.get() == TrafficSignalElement::UNKNOWN + : true; + + const auto last_detected_signal = + 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() == TrafficSignalElement::GREEN || + last_detected_signal.get() == TrafficSignalElement::AMBER) && + use_last_detect_color_; + + if (!current_is_not_red && !was_not_red) { + continue; + } + + non_red_lanelets.push_back(lanelet); + } + + return non_red_lanelets; +} + +uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal( + const lanelet::ConstLanelet & crosswalk, const lanelet::ConstLanelets & non_red_lanelets) const +{ + bool has_left_non_red_lane = false; + bool has_right_non_red_lane = false; + bool has_straight_non_red_lane = false; + bool has_related_non_red_tl = false; + + const std::string related_tl_id = crosswalk.attributeOr("related_traffic_light", "none"); + + for (const auto & lanelet : non_red_lanelets) { + const std::string turn_direction = lanelet.attributeOr("turn_direction", "none"); + + if (turn_direction == "left") { + has_left_non_red_lane = true; + } else if (turn_direction == "right") { + has_right_non_red_lane = true; + } else { + has_straight_non_red_lane = true; + } + + const auto tl_reg_elems = lanelet.regulatoryElementsAs(); + if (tl_reg_elems.front()->id() == std::atoi(related_tl_id.c_str())) { + has_related_non_red_tl = true; + } + } + + if (has_straight_non_red_lane || has_related_non_red_tl) { + 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 + ? TrafficSignalElement::RED + : TrafficSignalElement::UNKNOWN; +} + +boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal( + const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, + const TrafficLightIdMap & traffic_light_id_map) const +{ + boost::optional ret{boost::none}; + + double highest_confidence = 0.0; + for (const auto & traffic_light : traffic_lights) { + if (!traffic_light.isLineString()) { + continue; + } + + const int id = static_cast(traffic_light).id(); + if (traffic_light_id_map.count(id) == 0) { + continue; + } + + const auto & elements = traffic_light_id_map.at(id).first.elements; + if (elements.empty()) { + continue; + } + + const auto & color = elements.front().color; + const auto & confidence = elements.front().confidence; + if (confidence < highest_confidence) { + continue; + } + + highest_confidence = confidence; + ret = color; + } + + 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; +} + +void CrosswalkTrafficLightEstimatorNode::removeDuplicateIds(TrafficSignalArray & signal_array) const +{ + auto & signals = signal_array.traffic_light_groups; + std::sort(signals.begin(), signals.end(), [](const auto & s1, const auto & s2) { + return s1.traffic_light_group_id < s2.traffic_light_group_id; + }); + + signals.erase( + std::unique( + signals.begin(), signals.end(), + [](const auto & s1, const auto s2) { + return s1.traffic_light_group_id == s2.traffic_light_group_id; + }), + signals.end()); +} + +} // namespace autoware::crosswalk_traffic_light_estimator + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::crosswalk_traffic_light_estimator::CrosswalkTrafficLightEstimatorNode) diff --git a/perception/autoware_map_based_prediction/CMakeLists.txt b/perception/autoware_map_based_prediction/CMakeLists.txt new file mode 100644 index 0000000000000..9a1d9a1947c7c --- /dev/null +++ b/perception/autoware_map_based_prediction/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_map_based_prediction) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Eigen3 REQUIRED) + +find_package(glog REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +ament_auto_add_library(map_based_prediction_node SHARED + src/map_based_prediction_node.cpp + src/path_generator.cpp + src/debug.cpp +) + +target_link_libraries(map_based_prediction_node glog::glog) + +rclcpp_components_register_node(map_based_prediction_node + PLUGIN "autoware::map_based_prediction::MapBasedPredictionNode" + EXECUTABLE map_based_prediction +) + +## Tests +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(test_map_based_prediction ${test_files}) + + target_link_libraries(test_map_based_prediction + map_based_prediction_node + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/perception/map_based_prediction/README.md b/perception/autoware_map_based_prediction/README.md similarity index 100% rename from perception/map_based_prediction/README.md rename to perception/autoware_map_based_prediction/README.md diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml similarity index 100% rename from perception/map_based_prediction/config/map_based_prediction.param.yaml rename to perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml diff --git a/perception/map_based_prediction/images/exception.svg b/perception/autoware_map_based_prediction/images/exception.svg similarity index 100% rename from perception/map_based_prediction/images/exception.svg rename to perception/autoware_map_based_prediction/images/exception.svg diff --git a/perception/map_based_prediction/images/inside_road.svg b/perception/autoware_map_based_prediction/images/inside_road.svg similarity index 100% rename from perception/map_based_prediction/images/inside_road.svg rename to perception/autoware_map_based_prediction/images/inside_road.svg diff --git a/perception/map_based_prediction/images/outside_road.svg b/perception/autoware_map_based_prediction/images/outside_road.svg similarity index 100% rename from perception/map_based_prediction/images/outside_road.svg rename to perception/autoware_map_based_prediction/images/outside_road.svg diff --git a/perception/map_based_prediction/images/target_objects.svg b/perception/autoware_map_based_prediction/images/target_objects.svg similarity index 100% rename from perception/map_based_prediction/images/target_objects.svg rename to perception/autoware_map_based_prediction/images/target_objects.svg diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp similarity index 98% rename from perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp rename to perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 7a0010637c2f4..27c2a5079f441 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -51,7 +52,7 @@ #include #include -namespace map_based_prediction +namespace autoware::map_based_prediction { struct LateralKinematicsToLanelet { @@ -142,7 +143,8 @@ class MapBasedPredictionNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_markers_; rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Subscription::SharedPtr sub_traffic_signals_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_traffic_signals_{ + this, "/traffic_signals"}; // debug publisher std::unique_ptr> stop_watch_ptr_; @@ -416,6 +418,6 @@ class MapBasedPredictionNode : public rclcpp::Node return true; }; }; -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction #endif // MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp similarity index 98% rename from perception/map_based_prediction/include/map_based_prediction/path_generator.hpp rename to perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp index 0c1642600f4b1..bd098894f8a88 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -28,7 +28,7 @@ #include #include -namespace map_based_prediction +namespace autoware::map_based_prediction { using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; @@ -145,6 +145,6 @@ class PathGenerator const TrackedObject & object, const PosePath & ref_path, const double duration, const double speed_limit = 0.0) const; }; -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction #endif // MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ diff --git a/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml b/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml new file mode 100644 index 0000000000000..b65f0de892dcc --- /dev/null +++ b/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/perception/map_based_prediction/media/lane_change_detection.drawio.svg b/perception/autoware_map_based_prediction/media/lane_change_detection.drawio.svg similarity index 100% rename from perception/map_based_prediction/media/lane_change_detection.drawio.svg rename to perception/autoware_map_based_prediction/media/lane_change_detection.drawio.svg diff --git a/perception/map_based_prediction/media/lanechange_detection_right_to_left.png b/perception/autoware_map_based_prediction/media/lanechange_detection_right_to_left.png similarity index 100% rename from perception/map_based_prediction/media/lanechange_detection_right_to_left.png rename to perception/autoware_map_based_prediction/media/lanechange_detection_right_to_left.png diff --git a/perception/map_based_prediction/media/map_based_prediction_flow.drawio.svg b/perception/autoware_map_based_prediction/media/map_based_prediction_flow.drawio.svg similarity index 100% rename from perception/map_based_prediction/media/map_based_prediction_flow.drawio.svg rename to perception/autoware_map_based_prediction/media/map_based_prediction_flow.drawio.svg diff --git a/perception/map_based_prediction/media/object_history.drawio.svg b/perception/autoware_map_based_prediction/media/object_history.drawio.svg similarity index 100% rename from perception/map_based_prediction/media/object_history.drawio.svg rename to perception/autoware_map_based_prediction/media/object_history.drawio.svg diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml new file mode 100644 index 0000000000000..3671c1a01b0a2 --- /dev/null +++ b/perception/autoware_map_based_prediction/package.xml @@ -0,0 +1,38 @@ + + + + autoware_map_based_prediction + 0.1.0 + This package implements a map_based_prediction + Tomoya Kimura + Yoshi Ri + Takayuki Murooka + Kyoichi Sugahara + Kotaro Uetake + Apache License 2.0 + + ament_cmake + autoware_cmake + + autoware_perception_msgs + glog + interpolation + lanelet2_extension + motion_utils + rclcpp + rclcpp_components + tf2 + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_debug_msgs + unique_identifier_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/map_based_prediction/schema/map_based_prediction.schema.json b/perception/autoware_map_based_prediction/schema/map_based_prediction.schema.json similarity index 100% rename from perception/map_based_prediction/schema/map_based_prediction.schema.json rename to perception/autoware_map_based_prediction/schema/map_based_prediction.schema.json diff --git a/perception/autoware_map_based_prediction/src/debug.cpp b/perception/autoware_map_based_prediction/src/debug.cpp new file mode 100644 index 0000000000000..0c58d4aae1638 --- /dev/null +++ b/perception/autoware_map_based_prediction/src/debug.cpp @@ -0,0 +1,49 @@ +// Copyright 2021 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 "map_based_prediction/map_based_prediction_node.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" + +namespace autoware::map_based_prediction +{ +visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( + const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num) +{ + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + marker.ns = "maneuver"; + + marker.id = static_cast(obj_num); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = object.kinematics.pose_with_covariance.pose; + marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0); + + // Color by maneuver + double r = 0.0; + double g = 0.0; + double b = 0.0; + if (maneuver == Maneuver::LEFT_LANE_CHANGE) { + g = 1.0; + } else if (maneuver == Maneuver::RIGHT_LANE_CHANGE) { + r = 1.0; + } else { + b = 1.0; + } + marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.8); + + return marker; +} +} // namespace autoware::map_based_prediction diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp similarity index 99% rename from perception/map_based_prediction/src/map_based_prediction_node.cpp rename to perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index a00a43545cf23..20d532d7127d7 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -50,7 +50,7 @@ #include #include -namespace map_based_prediction +namespace autoware::map_based_prediction { namespace @@ -854,9 +854,6 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ sub_map_ = this->create_subscription( "/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&MapBasedPredictionNode::mapCallback, this, std::placeholders::_1)); - sub_traffic_signals_ = this->create_subscription( - "/traffic_signals", 1, - std::bind(&MapBasedPredictionNode::trafficSignalsCallback, this, std::placeholders::_1)); pub_objects_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); pub_debug_markers_ = @@ -948,6 +945,15 @@ void MapBasedPredictionNode::trafficSignalsCallback( void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects) { stop_watch_ptr_->toc("processing_time", true); + + // take traffic_signal + { + const auto msg = sub_traffic_signals_.takeData(); + if (msg) { + trafficSignalsCallback(msg); + } + } + // Guard for map pointer and frame transformation if (!lanelet_map_ptr_) { return; @@ -2495,7 +2501,7 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( return true; } -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction #include -RCLCPP_COMPONENTS_REGISTER_NODE(map_based_prediction::MapBasedPredictionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::map_based_prediction::MapBasedPredictionNode) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp similarity index 99% rename from perception/map_based_prediction/src/path_generator.cpp rename to perception/autoware_map_based_prediction/src/path_generator.cpp index 838d7b1c8e316..873219ffcf130 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -20,7 +20,7 @@ #include -namespace map_based_prediction +namespace autoware::map_based_prediction { PathGenerator::PathGenerator( const double sampling_time_interval, const double min_crosswalk_user_velocity) @@ -157,7 +157,7 @@ PredictedPath PathGenerator::generatePathForOnLaneVehicle( return generateStraightPath(object, duration); } - return generatePolynomialPath(object, ref_paths, speed_limit, duration, lateral_duration); + return generatePolynomialPath(object, ref_paths, duration, lateral_duration, speed_limit); } PredictedPath PathGenerator::generateStraightPath( @@ -487,4 +487,4 @@ FrenetPoint PathGenerator::getFrenetPoint( return frenet_point; } -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction diff --git a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp b/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp similarity index 84% rename from perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp rename to perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp index 518b28a555849..08e5b4977e583 100644 --- a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp +++ b/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp @@ -54,8 +54,9 @@ TEST(PathGenerator, test_generatePathForNonVehicleObject) const double prediction_time_horizon = 10.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate pedestrian object TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); @@ -77,8 +78,9 @@ TEST(PathGenerator, test_generatePathForLowSpeedVehicle) const double prediction_time_horizon = 10.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); @@ -100,8 +102,9 @@ TEST(PathGenerator, test_generatePathForOffLaneVehicle) const double prediction_time_horizon = 10.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); @@ -123,14 +126,15 @@ TEST(PathGenerator, test_generatePathForOnLaneVehicle) const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); // Generate reference path - map_based_prediction::PosePath ref_paths; + autoware::map_based_prediction::PosePath ref_paths; geometry_msgs::msg::Pose pose; pose.position.x = 0.0; pose.position.y = 0.0; @@ -154,14 +158,15 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) const double prediction_time_horizon = 10.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); // Generate dummy crosswalk - map_based_prediction::CrosswalkEdgePoints reachable_crosswalk; + autoware::map_based_prediction::CrosswalkEdgePoints reachable_crosswalk; reachable_crosswalk.front_center_point << 0.0, 0.0; reachable_crosswalk.front_right_point << 1.0, 0.0; reachable_crosswalk.front_left_point << -1.0, 0.0; @@ -185,8 +190,9 @@ TEST(PathGenerator, test_generatePathToTargetPoint) // Generate Path generator const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); diff --git a/perception/map_based_prediction/test/test_map_based_prediction.cpp b/perception/autoware_map_based_prediction/test/test_map_based_prediction.cpp similarity index 100% rename from perception/map_based_prediction/test/test_map_based_prediction.cpp rename to perception/autoware_map_based_prediction/test/test_map_based_prediction.cpp diff --git a/perception/crosswalk_traffic_light_estimator/CMakeLists.txt b/perception/crosswalk_traffic_light_estimator/CMakeLists.txt deleted file mode 100644 index 2947070eca01e..0000000000000 --- a/perception/crosswalk_traffic_light_estimator/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(crosswalk_traffic_light_estimator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -########### -## Build ## -########### - -include_directories() - -ament_auto_add_library(crosswalk_traffic_light_estimator SHARED - src/node.cpp -) - -rclcpp_components_register_node(crosswalk_traffic_light_estimator - PLUGIN "traffic_light::CrosswalkTrafficLightEstimatorNode" - EXECUTABLE crosswalk_traffic_light_estimator_node -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -############# -## Install ## -############# - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) 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 deleted file mode 100644 index 5072f8b8fc296..0000000000000 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ /dev/null @@ -1,122 +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 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 -#include - -#include -#include -#include -#include -#include -namespace traffic_light -{ - -using autoware_map_msgs::msg::LaneletMapBin; -using autoware_planning_msgs::msg::LaneletRoute; -using tier4_autoware_utils::DebugPublisher; -using tier4_autoware_utils::StopWatch; -using tier4_debug_msgs::msg::Float64Stamped; -using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; -using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; -using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; -using TrafficSignalAndTime = std::pair; -using TrafficLightIdMap = std::unordered_map; - -using TrafficLightIdArray = std::unordered_map>; - -class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node -{ -public: - explicit CrosswalkTrafficLightEstimatorNode(const rclcpp::NodeOptions & options); - -private: - rclcpp::Subscription::SharedPtr sub_map_; - rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Subscription::SharedPtr sub_traffic_light_array_; - rclcpp::Publisher::SharedPtr pub_traffic_light_array_; - - lanelet::LaneletMapPtr lanelet_map_ptr_; - lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; - lanelet::routing::RoutingGraphPtr routing_graph_ptr_; - std::shared_ptr overall_graphs_ptr_; - - lanelet::ConstLanelets conflicting_crosswalks_; - - void onMap(const LaneletMapBin::ConstSharedPtr msg); - void onRoute(const LaneletRoute::ConstSharedPtr msg); - void onTrafficLightArray(const TrafficSignalArray::ConstSharedPtr msg); - - void updateLastDetectedSignal(const TrafficLightIdMap & traffic_signals); - void updateLastDetectedSignals(const TrafficLightIdMap & traffic_signals); - void updateFlashingState(const TrafficSignal & signal); - uint8_t updateAndGetColorState(const TrafficSignal & signal); - void setCrosswalkTrafficSignal( - const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, - TrafficSignalArray & output); - - lanelet::ConstLanelets getNonRedLanelets( - const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const; - - uint8_t estimateCrosswalkTrafficSignal( - const lanelet::ConstLanelet & crosswalk, const lanelet::ConstLanelets & non_red_lanelets) const; - - boost::optional getHighestConfidenceTrafficSignal( - 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; - - void removeDuplicateIds(TrafficSignalArray & signal_array) const; - - // Node param - bool use_last_detect_color_; - double last_detect_color_hold_time_; - double last_colors_hold_time_; - - // Signal history - TrafficLightIdMap last_detect_color_; - TrafficLightIdArray last_colors_; - - // State - std::map is_flashing_; - std::map current_color_state_; - - // Stop watch - StopWatch stop_watch_; - - // Debug - std::shared_ptr pub_processing_time_; -}; - -} // namespace traffic_light - -#endif // CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml deleted file mode 100644 index 123b721c40e0a..0000000000000 --- a/perception/crosswalk_traffic_light_estimator/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - crosswalk_traffic_light_estimator - 0.1.0 - The crosswalk_traffic_light_estimator package - - Satoshi Ota - Shunsuke Miura - Tao Zhong - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_map_msgs - autoware_perception_msgs - autoware_planning_msgs - lanelet2_extension - rclcpp - rclcpp_components - tier4_autoware_utils - tier4_perception_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp deleted file mode 100644 index 2a760b3f00e4e..0000000000000 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ /dev/null @@ -1,545 +0,0 @@ -// 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. -// 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 "crosswalk_traffic_light_estimator/node.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace traffic_light -{ -namespace -{ - -bool hasMergeLane( - const lanelet::ConstLanelet & lanelet_1, const lanelet::ConstLanelet & lanelet_2, - const lanelet::routing::RoutingGraphPtr & routing_graph_ptr) -{ - const auto next_lanelets_1 = routing_graph_ptr->following(lanelet_1); - const auto next_lanelets_2 = routing_graph_ptr->following(lanelet_2); - - for (const auto & next_lanelet_1 : next_lanelets_1) { - for (const auto & next_lanelet_2 : next_lanelets_2) { - if (next_lanelet_1.id() == next_lanelet_2.id()) { - return true; - } - } - } - - return false; -} - -bool hasMergeLane( - const lanelet::ConstLanelets & lanelets, - const lanelet::routing::RoutingGraphPtr & routing_graph_ptr) -{ - for (size_t i = 0; i < lanelets.size(); ++i) { - for (size_t j = i + 1; j < lanelets.size(); ++j) { - const auto lanelet_1 = lanelets.at(i); - const auto lanelet_2 = lanelets.at(j); - - if (lanelet_1.id() == lanelet_2.id()) { - continue; - } - - const std::string turn_direction_1 = lanelet_1.attributeOr("turn_direction", "none"); - const std::string turn_direction_2 = lanelet_2.attributeOr("turn_direction", "none"); - if (turn_direction_1 == turn_direction_2) { - continue; - } - - if (!hasMergeLane(lanelet_1, lanelet_2, routing_graph_ptr)) { - continue; - } - - return true; - } - } - - return false; -} -} // namespace - -CrosswalkTrafficLightEstimatorNode::CrosswalkTrafficLightEstimatorNode( - const rclcpp::NodeOptions & options) -: Node("crosswalk_traffic_light_estimator", options) -{ - using std::placeholders::_1; - - use_last_detect_color_ = declare_parameter("use_last_detect_color"); - last_detect_color_hold_time_ = declare_parameter("last_detect_color_hold_time"); - last_colors_hold_time_ = declare_parameter("last_colors_hold_time"); - - sub_map_ = create_subscription( - "~/input/vector_map", rclcpp::QoS{1}.transient_local(), - std::bind(&CrosswalkTrafficLightEstimatorNode::onMap, this, _1)); - sub_route_ = create_subscription( - "~/input/route", rclcpp::QoS{1}.transient_local(), - std::bind(&CrosswalkTrafficLightEstimatorNode::onRoute, this, _1)); - sub_traffic_light_array_ = create_subscription( - "~/input/classified/traffic_signals", rclcpp::QoS{1}, - std::bind(&CrosswalkTrafficLightEstimatorNode::onTrafficLightArray, this, _1)); - - pub_traffic_light_array_ = - this->create_publisher("~/output/traffic_signals", rclcpp::QoS{1}); - pub_processing_time_ = std::make_shared(this, "~/debug"); -} - -void CrosswalkTrafficLightEstimatorNode::onMap(const LaneletMapBin::ConstSharedPtr msg) -{ - RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Start loading lanelet"); - lanelet_map_ptr_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg( - *msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); - const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( - lanelet::Locations::Germany, lanelet::Participants::Vehicle); - const auto pedestrian_rules = lanelet::traffic_rules::TrafficRulesFactory::create( - lanelet::Locations::Germany, lanelet::Participants::Pedestrian); - lanelet::routing::RoutingGraphConstPtr vehicle_graph = - lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *traffic_rules); - lanelet::routing::RoutingGraphConstPtr pedestrian_graph = - lanelet::routing::RoutingGraph::build(*lanelet_map_ptr_, *pedestrian_rules); - lanelet::routing::RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); - overall_graphs_ptr_ = - std::make_shared(overall_graphs); - RCLCPP_DEBUG(get_logger(), "[CrosswalkTrafficLightEstimatorNode]: Map is loaded"); -} - -void CrosswalkTrafficLightEstimatorNode::onRoute(const LaneletRoute::ConstSharedPtr msg) -{ - if (lanelet_map_ptr_ == nullptr) { - RCLCPP_WARN(get_logger(), "cannot set traffic light in route because don't receive map"); - return; - } - - lanelet::ConstLanelets route_lanelets; - for (const auto & segment : msg->segments) { - for (const auto & primitive : segment.primitives) { - try { - route_lanelets.push_back(lanelet_map_ptr_->laneletLayer.get(primitive.id)); - } catch (const lanelet::NoSuchPrimitiveError & ex) { - RCLCPP_ERROR(get_logger(), "%s", ex.what()); - return; - } - } - } - - conflicting_crosswalks_.clear(); - - for (const auto & route_lanelet : route_lanelets) { - constexpr int PEDESTRIAN_GRAPH_ID = 1; - const auto conflict_lls = - overall_graphs_ptr_->conflictingInGraph(route_lanelet, PEDESTRIAN_GRAPH_ID); - for (const auto & lanelet : conflict_lls) { - conflicting_crosswalks_.push_back(lanelet); - } - } -} - -void CrosswalkTrafficLightEstimatorNode::onTrafficLightArray( - const TrafficSignalArray::ConstSharedPtr msg) -{ - StopWatch stop_watch; - stop_watch.tic("Total"); - - TrafficSignalArray output = *msg; - - TrafficLightIdMap traffic_light_id_map; - for (const auto & traffic_signal : msg->traffic_light_groups) { - traffic_light_id_map[traffic_signal.traffic_light_group_id] = - std::pair(traffic_signal, get_clock()->now()); - } - for (const auto & crosswalk : conflicting_crosswalks_) { - constexpr int VEHICLE_GRAPH_ID = 0; - const auto conflict_lls = overall_graphs_ptr_->conflictingInGraph(crosswalk, VEHICLE_GRAPH_ID); - const auto non_red_lanelets = getNonRedLanelets(conflict_lls, traffic_light_id_map); - - const auto crosswalk_tl_color = estimateCrosswalkTrafficSignal(crosswalk, non_red_lanelets); - setCrosswalkTrafficSignal(crosswalk, crosswalk_tl_color, *msg, output); - } - - removeDuplicateIds(output); - - updateLastDetectedSignal(traffic_light_id_map); - updateLastDetectedSignals(traffic_light_id_map); - - pub_traffic_light_array_->publish(output); - pub_processing_time_->publish("processing_time_ms", stop_watch.toc("Total")); - - return; -} - -void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignal( - const TrafficLightIdMap & traffic_light_id_map) -{ - for (const auto & input_traffic_signal : traffic_light_id_map) { - const auto & elements = input_traffic_signal.second.first.elements; - - if (elements.empty()) { - continue; - } - - if (elements.front().color == TrafficSignalElement::UNKNOWN) { - continue; - } - - const auto & id = input_traffic_signal.second.first.traffic_light_group_id; - - if (last_detect_color_.count(id) == 0) { - last_detect_color_.insert(std::make_pair(id, input_traffic_signal.second)); - continue; - } - - last_detect_color_.at(id) = input_traffic_signal.second; - } - - std::vector erase_id_list; - for (auto & last_traffic_signal : last_detect_color_) { - const auto & id = last_traffic_signal.second.first.traffic_light_group_id; - - if (traffic_light_id_map.count(id) == 0) { - // hold signal recognition results for [last_detect_color_hold_time_] seconds. - const auto time_from_last_detected = - (get_clock()->now() - last_traffic_signal.second.second).seconds(); - if (time_from_last_detected > last_detect_color_hold_time_) { - erase_id_list.emplace_back(id); - } - } - } - for (const auto id : erase_id_list) { - last_detect_color_.erase(id); - is_flashing_.erase(id); - current_color_state_.erase(id); - } -} - -void CrosswalkTrafficLightEstimatorNode::updateLastDetectedSignals( - const TrafficLightIdMap & traffic_light_id_map) -{ - for (const auto & input_traffic_signal : traffic_light_id_map) { - const auto & elements = input_traffic_signal.second.first.elements; - - if (elements.empty()) { - continue; - } - - if ( - elements.front().color == TrafficSignalElement::UNKNOWN && elements.front().confidence == 1) { - continue; - } - - const auto & id = input_traffic_signal.second.first.traffic_light_group_id; - - if (last_colors_.count(id) == 0) { - std::vector signal{input_traffic_signal.second}; - last_colors_.insert(std::make_pair(id, signal)); - continue; - } - - last_colors_.at(id).push_back(input_traffic_signal.second); - } - - std::vector erase_id_list; - for (auto & last_traffic_signal : last_colors_) { - const auto & id = last_traffic_signal.first; - for (auto it = last_traffic_signal.second.begin(); it != last_traffic_signal.second.end();) { - auto sig = (*it).first; - rclcpp::Time t = (*it).second; - - // hold signal recognition results for [last_colors_hold_time_] seconds. - const auto time_from_last_detected = (get_clock()->now() - t).seconds(); - if (time_from_last_detected > last_colors_hold_time_) { - it = last_traffic_signal.second.erase(it); - } else { - ++it; - } - } - if (last_traffic_signal.second.empty()) { - erase_id_list.emplace_back(id); - } - } - for (const auto id : erase_id_list) last_colors_.erase(id); -} - -void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( - const lanelet::ConstLanelet & crosswalk, const uint8_t color, const TrafficSignalArray & msg, - TrafficSignalArray & output) -{ - const auto tl_reg_elems = crosswalk.regulatoryElementsAs(); - - std::unordered_map valid_id2idx_map; // detected traffic light - - for (size_t i = 0; i < msg.traffic_light_groups.size(); ++i) { - auto signal = msg.traffic_light_groups[i]; - valid_id2idx_map[signal.traffic_light_group_id] = i; - } - - for (const auto & tl_reg_elem : tl_reg_elems) { - auto id = tl_reg_elem->id(); - // if valid prediction exists, overwrite the estimation; else, use the estimation - if (valid_id2idx_map.count(id)) { - size_t idx = valid_id2idx_map[id]; - auto signal = msg.traffic_light_groups[idx]; - updateFlashingState(signal); // check if it is flashing - // update output msg according to flashing and current state - output.traffic_light_groups[idx].elements[0].color = updateAndGetColorState(signal); - } else { - 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_light_group_id = id; - output.traffic_light_groups.push_back(output_traffic_signal); - } - } -} - -void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal) -{ - const auto id = signal.traffic_light_group_id; - - // no record of detected color in last_detect_color_hold_time_ - if (is_flashing_.count(id) == 0) { - is_flashing_.insert(std::make_pair(id, false)); - return; - } - - // flashing green - if ( - signal.elements.front().color == TrafficSignalElement::UNKNOWN && - signal.elements.front().confidence != 0 && // not due to occlusion - current_color_state_.at(id) != TrafficSignalElement::UNKNOWN) { - is_flashing_.at(id) = true; - return; - } - - // history exists - if (last_colors_.count(id) > 0) { - std::vector history = last_colors_.at(id); - for (const auto & h : history) { - if (h.first.elements.front().color != signal.elements.front().color) { - // keep the current value if not same with input signal - return; - } - } - // all history is same with input signal - is_flashing_.at(id) = false; - } - - // no record of detected color in last_color_hold_time_ - // keep the current value - return; -} - -uint8_t CrosswalkTrafficLightEstimatorNode::updateAndGetColorState(const TrafficSignal & signal) -{ - const auto id = signal.traffic_light_group_id; - const auto color = signal.elements[0].color; - - if (current_color_state_.count(id) == 0) { - current_color_state_.insert(std::make_pair(id, color)); - } else if (is_flashing_.at(id) == false) { - current_color_state_.at(id) = color; - } else if (is_flashing_.at(id) == true) { - if ( - current_color_state_.at(id) == TrafficSignalElement::GREEN && - color == TrafficSignalElement::RED) { - current_color_state_.at(id) = TrafficSignalElement::RED; - } else if ( - current_color_state_.at(id) == TrafficSignalElement::RED && - color == TrafficSignalElement::GREEN) { - current_color_state_.at(id) = TrafficSignalElement::GREEN; - } else if (current_color_state_.at(id) == TrafficSignalElement::UNKNOWN) { - if (color == TrafficSignalElement::GREEN || color == TrafficSignalElement::UNKNOWN) - current_color_state_.at(id) = TrafficSignalElement::GREEN; - if (color == TrafficSignalElement::RED) - current_color_state_.at(id) = TrafficSignalElement::RED; - } - } - - return current_color_state_.at(id); -} - -lanelet::ConstLanelets CrosswalkTrafficLightEstimatorNode::getNonRedLanelets( - const lanelet::ConstLanelets & lanelets, const TrafficLightIdMap & traffic_light_id_map) const -{ - lanelet::ConstLanelets non_red_lanelets{}; - - for (const auto & lanelet : lanelets) { - const auto tl_reg_elems = lanelet.regulatoryElementsAs(); - - if (tl_reg_elems.empty()) { - continue; - } - - const auto tl_reg_elem = tl_reg_elems.front(); - const auto current_detected_signal = - 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() == TrafficSignalElement::GREEN || - current_detected_signal.get() == TrafficSignalElement::AMBER - : true; - - const auto current_is_unknown_or_none = - current_detected_signal ? current_detected_signal.get() == TrafficSignalElement::UNKNOWN - : true; - - const auto last_detected_signal = - 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() == TrafficSignalElement::GREEN || - last_detected_signal.get() == TrafficSignalElement::AMBER) && - use_last_detect_color_; - - if (!current_is_not_red && !was_not_red) { - continue; - } - - non_red_lanelets.push_back(lanelet); - } - - return non_red_lanelets; -} - -uint8_t CrosswalkTrafficLightEstimatorNode::estimateCrosswalkTrafficSignal( - const lanelet::ConstLanelet & crosswalk, const lanelet::ConstLanelets & non_red_lanelets) const -{ - bool has_left_non_red_lane = false; - bool has_right_non_red_lane = false; - bool has_straight_non_red_lane = false; - bool has_related_non_red_tl = false; - - const std::string related_tl_id = crosswalk.attributeOr("related_traffic_light", "none"); - - for (const auto & lanelet : non_red_lanelets) { - const std::string turn_direction = lanelet.attributeOr("turn_direction", "none"); - - if (turn_direction == "left") { - has_left_non_red_lane = true; - } else if (turn_direction == "right") { - has_right_non_red_lane = true; - } else { - has_straight_non_red_lane = true; - } - - const auto tl_reg_elems = lanelet.regulatoryElementsAs(); - if (tl_reg_elems.front()->id() == std::atoi(related_tl_id.c_str())) { - has_related_non_red_tl = true; - } - } - - if (has_straight_non_red_lane || has_related_non_red_tl) { - 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 - ? TrafficSignalElement::RED - : TrafficSignalElement::UNKNOWN; -} - -boost::optional CrosswalkTrafficLightEstimatorNode::getHighestConfidenceTrafficSignal( - const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, - const TrafficLightIdMap & traffic_light_id_map) const -{ - boost::optional ret{boost::none}; - - double highest_confidence = 0.0; - for (const auto & traffic_light : traffic_lights) { - if (!traffic_light.isLineString()) { - continue; - } - - const int id = static_cast(traffic_light).id(); - if (traffic_light_id_map.count(id) == 0) { - continue; - } - - const auto & elements = traffic_light_id_map.at(id).first.elements; - if (elements.empty()) { - continue; - } - - const auto & color = elements.front().color; - const auto & confidence = elements.front().confidence; - if (confidence < highest_confidence) { - continue; - } - - highest_confidence = confidence; - ret = color; - } - - 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; -} - -void CrosswalkTrafficLightEstimatorNode::removeDuplicateIds(TrafficSignalArray & signal_array) const -{ - auto & signals = signal_array.traffic_light_groups; - std::sort(signals.begin(), signals.end(), [](const auto & s1, const auto & s2) { - return s1.traffic_light_group_id < s2.traffic_light_group_id; - }); - - signals.erase( - std::unique( - signals.begin(), signals.end(), - [](const auto & s1, const auto s2) { - return s1.traffic_light_group_id == s2.traffic_light_group_id; - }), - signals.end()); -} - -} // namespace traffic_light - -#include - -RCLCPP_COMPONENTS_REGISTER_NODE(traffic_light::CrosswalkTrafficLightEstimatorNode) diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index d97bbaa2118e5..314c9cfb97a75 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -18,7 +18,7 @@ #include "pointcloud_preprocessor/filter.hpp" #include "pointcloud_preprocessor/transform_info.hpp" -#include +#include #include @@ -43,7 +43,7 @@ class ScanGroundFilterTest; namespace ground_segmentation { -using vehicle_info_util::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfo; class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter { diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.py b/perception/ground_segmentation/launch/scan_ground_filter.launch.py index d3d856925b9db..df8290997315e 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.py +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.py @@ -85,7 +85,7 @@ def add_launch_arg(name: str, default_value=None): return DeclareLaunchArgument(name, default_value=default_value) default_vehicle_info_param = os.path.join( - get_package_share_directory("vehicle_info_util"), "config/vehicle_info.param.yaml" + get_package_share_directory("autoware_vehicle_info_utils"), "config/vehicle_info.param.yaml" ) vehicle_info_param = DeclareLaunchArgument( diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml index 2a9e4983ecb56..1146138211211 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/perception/ground_segmentation/package.xml b/perception/ground_segmentation/package.xml index 05d5d18baf29e..1b807b393b739 100644 --- a/perception/ground_segmentation/package.xml +++ b/perception/ground_segmentation/package.xml @@ -19,6 +19,7 @@ ament_cmake_auto autoware_cmake + autoware_vehicle_info_utils libopencv-dev pcl_conversions pcl_ros @@ -31,7 +32,6 @@ tf2_eigen tf2_ros tf2_sensor_msgs - vehicle_info_util yaml-cpp ament_lint_auto diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index a63d218df62de..e638baba12521 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -14,10 +14,10 @@ #include "ground_segmentation/scan_ground_filter_nodelet.hpp" +#include #include #include #include -#include #include #include @@ -25,12 +25,12 @@ namespace ground_segmentation { +using autoware::vehicle_info_utils::VehicleInfoUtils; using pointcloud_preprocessor::get_param; using tier4_autoware_utils::calcDistance3d; using tier4_autoware_utils::deg2rad; using tier4_autoware_utils::normalizeDegree; using tier4_autoware_utils::normalizeRadian; -using vehicle_info_util::VehicleInfoUtil; ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("ScanGroundFilter", options) @@ -59,7 +59,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); use_lowest_point_ = declare_parameter("use_lowest_point"); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); - vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = VehicleInfoUtils(*this).getVehicleInfo(); grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; // changing the mode of grid division diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 4168c483469ab..c458c17bed793 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -48,6 +48,13 @@ class SegmentPointCloudFusionNode : public FusionNode 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) { + int point_step = input_pointcloud_msg.point_step; + int x_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "x")].offset; + int y_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset; + int z_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; + size_t output_pointcloud_size = 0; + output_cloud.data.clear(); + output_cloud.data.resize(input_pointcloud_msg.data.size()); + output_cloud.fields = input_pointcloud_msg.fields; + output_cloud.header = input_pointcloud_msg.header; + output_cloud.height = input_pointcloud_msg.height; + output_cloud.point_step = input_pointcloud_msg.point_step; + output_cloud.is_bigendian = input_pointcloud_msg.is_bigendian; + output_cloud.is_dense = input_pointcloud_msg.is_dense; + for (size_t global_offset = 0; global_offset < transformed_cloud.data.size(); + global_offset += point_step) { + float transformed_x = + *reinterpret_cast(&transformed_cloud.data[global_offset + x_offset]); + float transformed_y = + *reinterpret_cast(&transformed_cloud.data[global_offset + y_offset]); + float transformed_z = + *reinterpret_cast(&transformed_cloud.data[global_offset + z_offset]); // skip filtering pointcloud behind the camera or too far from camera - if (*iter_z <= 0.0 || *iter_z > filter_distance_threshold_) { - output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + if (transformed_z <= 0.0 || transformed_z > filter_distance_threshold_) { + copyPointCloud( + input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); continue; } - Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector4d projected_point = + projection * Eigen::Vector4d(transformed_x, transformed_y, transformed_z, 1.0); Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); @@ -107,7 +122,8 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( normalized_projected_point.x() > 0 && normalized_projected_point.x() < camera_info.width && normalized_projected_point.y() > 0 && normalized_projected_point.y() < camera_info.height; if (!is_inside_image) { - output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + copyPointCloud( + input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); continue; } @@ -116,16 +132,19 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( static_cast(normalized_projected_point.y()), static_cast(normalized_projected_point.x())); if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { - output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + copyPointCloud( + input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); continue; } if (!filter_semantic_label_target_.at(semantic_id)) { - output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + copyPointCloud( + input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); } } - pcl::toROSMsg(output_cloud, output_pointcloud_msg); - output_pointcloud_msg.header = input_pointcloud_msg.header; + output_cloud.data.resize(output_pointcloud_size); + output_cloud.row_step = output_pointcloud_size / output_cloud.height; + output_cloud.width = output_pointcloud_size / output_cloud.point_step / output_cloud.height; } bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) diff --git a/perception/lidar_transfusion/CMakeLists.txt b/perception/lidar_transfusion/CMakeLists.txt new file mode 100644 index 0000000000000..c0e2d85f27e62 --- /dev/null +++ b/perception/lidar_transfusion/CMakeLists.txt @@ -0,0 +1,156 @@ +cmake_minimum_required(VERSION 3.14) +project(lidar_transfusion) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +add_compile_options(-Wno-deprecated-declarations) + +option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) + +# set flags for CUDA availability +option(CUDA_AVAIL "CUDA available" OFF) +find_package(CUDA) +if(CUDA_FOUND) + find_library(CUBLAS_LIBRARIES cublas HINTS + ${CUDA_TOOLKIT_ROOT_DIR}/lib64 + ${CUDA_TOOLKIT_ROOT_DIR}/lib + ) + if(CUDA_VERBOSE) + message("CUDA is available!") + message("CUDA Libs: ${CUDA_LIBRARIES}") + message("CUDA Headers: ${CUDA_INCLUDE_DIRS}") + endif() + # Note: cublas_device was depreciated in CUDA version 9.2 + # https://forums.developer.nvidia.com/t/where-can-i-find-libcublas-device-so-or-libcublas-device-a/67251/4 + # In LibTorch, CUDA_cublas_device_LIBRARY is used. + unset(CUDA_cublas_device_LIBRARY CACHE) + set(CUDA_AVAIL ON) +else() + message("CUDA NOT FOUND") + set(CUDA_AVAIL OFF) +endif() + +# set flags for TensorRT availability +option(TRT_AVAIL "TensorRT available" OFF) +# try to find the tensorRT modules +find_library(NVINFER nvinfer) +find_library(NVONNXPARSER nvonnxparser) +if(NVINFER AND NVONNXPARSER) + if(CUDA_VERBOSE) + message("TensorRT is available!") + message("NVINFER: ${NVINFER}") + message("NVONNXPARSER: ${NVONNXPARSER}") + endif() + set(TRT_AVAIL ON) +else() + message("TensorRT is NOT Available") + set(TRT_AVAIL OFF) +endif() + +# set flags for CUDNN availability +option(CUDNN_AVAIL "CUDNN available" OFF) +# try to find the CUDNN module +find_library(CUDNN_LIBRARY +NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name} +PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX} +PATH_SUFFIXES lib lib64 bin +DOC "CUDNN library." +) +if(CUDNN_LIBRARY) + if(CUDA_VERBOSE) + message(STATUS "CUDNN is available!") + message(STATUS "CUDNN_LIBRARY: ${CUDNN_LIBRARY}") + endif() + set(CUDNN_AVAIL ON) +else() + message("CUDNN is NOT Available") + set(CUDNN_AVAIL OFF) +endif() + +if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + include_directories( + include + ${CUDA_INCLUDE_DIRS} + ) + + if(CMAKE_BUILD_TYPE STREQUAL "Debug") + set(CMAKE_CUDA_FLAGS ${CMAKE_CUDA_FLAGS} "-g -G") + set(CUDA_NVCC_FLAGS "-g -G") + endif() + + ament_auto_add_library(transfusion_lib SHARED + lib/detection_class_remapper.cpp + lib/network/network_trt.cpp + lib/postprocess/non_maximum_suppression.cpp + lib/preprocess/voxel_generator.cpp + lib/preprocess/pointcloud_densification.cpp + lib/ros_utils.cpp + lib/transfusion_trt.cpp + ) + + cuda_add_library(transfusion_cuda_lib SHARED + lib/postprocess/circle_nms_kernel.cu + lib/postprocess/postprocess_kernel.cu + lib/preprocess/preprocess_kernel.cu + ) + + target_link_libraries(transfusion_lib + ${NVINFER} + ${NVONNXPARSER} + ${CUDA_LIBRARIES} + ${CUBLAS_LIBRARIES} + ${CUDA_curand_LIBRARY} + ${CUDNN_LIBRARY} + transfusion_cuda_lib + ) + + target_include_directories(transfusion_lib + PUBLIC + $ + $ + ) + + # To suppress unknown-pragmas error. The root-cause is CUB library in CUDA 11.6. + # This issue was fixed by https://github.com/NVIDIA/cub/commit/7d608bf1dc14553e2fb219eabeed80b76621b6fe + target_include_directories(transfusion_lib + SYSTEM PUBLIC + ${CUDA_INCLUDE_DIRS} + ) + + ament_auto_add_library(lidar_transfusion_component SHARED + src/lidar_transfusion_node.cpp + ) + + target_link_libraries(lidar_transfusion_component + transfusion_lib + ) + + rclcpp_components_register_node(lidar_transfusion_component + PLUGIN "lidar_transfusion::LidarTransfusionNode" + EXECUTABLE lidar_transfusion_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 + ) + +else() + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + ament_auto_package( + INSTALL_TO_SHARE + launch + ) +endif() diff --git a/perception/lidar_transfusion/README.md b/perception/lidar_transfusion/README.md new file mode 100644 index 0000000000000..7974714720c32 --- /dev/null +++ b/perception/lidar_transfusion/README.md @@ -0,0 +1,113 @@ +# lidar_transfusion + +## Purpose + +The `lidar_transfusion` package is used for 3D object detection based on lidar data (x, y, z, intensity). + +## Inner-workings / Algorithms + +The implementation bases on TransFusion [1] work. It uses TensorRT library for data process and network inference. + +We trained the models using . + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------- | ------------------------------- | ----------------- | +| `~/input/pointcloud` | `sensor_msgs::msg::PointCloud2` | Input pointcloud. | + +### Output + +| Name | Type | Description | +| -------------------------------------- | ------------------------------------------------ | --------------------------- | +| `~/output/objects` | `autoware_perception_msgs::msg::DetectedObjects` | Detected objects. | +| `debug/cyclic_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Cyclic time (ms). | +| `debug/pipeline_latency_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Pipeline latency time (ms). | +| `debug/processing_time/preprocess_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Preprocess (ms). | +| `debug/processing_time/inference_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Inference time (ms). | +| `debug/processing_time/postprocess_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Postprocess time (ms). | +| `debug/processing_time/total_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Total processing time (ms). | + +## Parameters + +### TransFusion + +{{ json_to_markdown("perception/lidar_transfusion/schema/transfusion.schema.json") }} + +### Detection class remapper + +{{ json_to_markdown("perception/lidar_transfusion/schema/detection_class_remapper.schema.json") }} + +### The `build_only` option + +The `lidar_transfusion` node has `build_only` option to build the TensorRT engine file from the ONNX file. +Although it is preferred to move all the ROS parameters in `.param.yaml` file in Autoware Universe, the `build_only` option is not moved to the `.param.yaml` file for now, because it may be used as a flag to execute the build as a pre-task. You can execute with the following command: + +```bash +ros2 launch lidar_transfusion lidar_transfusion.launch.xml build_only:=true +``` + +### The `log_level` option + +The default logging severity level for `lidar_transfusion` is `info`. For debugging purposes, the developer may decrease severity level using `log_level` parameter: + +```bash +ros2 launch lidar_transfusion lidar_transfusion.launch.xml log_level:=debug +``` + +## Assumptions / Known limits + +## Trained Models + +You can download the onnx format of trained models by clicking on the links below. + +- TransFusion: [transfusion.onnx](https://awf.ml.dev.web.auto/perception/models/transfusion/v1/transfusion.onnx) + +The model was trained in TIER IV's internal database (~11k lidar frames) for 20 epochs. + +### Changelog + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## References/External links + +[1] Xuyang Bai, Zeyu Hu, Xinge Zhu, Qingqiu Huang, Yilun Chen, Hongbo Fu and Chiew-Lan Tai. "TransFusion: Robust LiDAR-Camera Fusion for 3D Object Detection with Transformers." arXiv preprint arXiv:2203.11496 (2022). + +[2] + +[3] + +[4] + +[5] + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/lidar_transfusion/config/detection_class_remapper.param.yaml b/perception/lidar_transfusion/config/detection_class_remapper.param.yaml new file mode 100644 index 0000000000000..baea087c96bca --- /dev/null +++ b/perception/lidar_transfusion/config/detection_class_remapper.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + allow_remapping_by_area_matrix: + # NOTE(kl): We turn all vehicles into trailers if they go over 3x12 [m^2]. + # NOTE(kl): We turn cars into trucks if they have an area between 2.2 x 5.5 and 3.0 * 12.0 [m^2] + # row: original class. column: class to remap to + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 0, 1, 0, 1, 0, 0, 0, #CAR + 0, 0, 0, 0, 1, 0, 0, 0, #TRUCK + 0, 0, 0, 0, 1, 0, 0, 0, #BUS + 0, 0, 0, 0, 0, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 0, 0, 0, #MOTORBIKE + 0, 0, 0, 0, 0, 0, 0, 0, #BICYCLE + 0, 0, 0, 0, 0, 0, 0, 0] #PEDESTRIAN + + min_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 12.100, 0.000, 36.000, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN + + + max_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 36.000, 0.000, 999.999, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN diff --git a/perception/lidar_transfusion/config/transfusion.param.yaml b/perception/lidar_transfusion/config/transfusion.param.yaml new file mode 100644 index 0000000000000..feabe359caf1f --- /dev/null +++ b/perception/lidar_transfusion/config/transfusion.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + # network + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + trt_precision: fp16 + voxels_num: [5000, 30000, 60000] # [min, opt, max] + point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] # [x_min, y_min, z_min, x_max, y_max, z_max] + voxel_size: [0.3, 0.3, 8.0] # [x, y, z] + onnx_path: "$(var model_path)/transfusion.onnx" + engine_path: "$(var model_path)/transfusion.engine" + # pre-process params + densification_num_past_frames: 1 + densification_world_frame_id: map + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] # refers to the class_names + score_threshold: 0.2 diff --git a/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp new file mode 100644 index 0000000000000..5c25a936d5392 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp @@ -0,0 +1,126 @@ +// Copyright 2024 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) 2020, NVIDIA CORPORATION. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +/* + * This code is licensed under CC0 1.0 Universal (Public Domain). + * You can use this without any limitation. + * https://creativecommons.org/publicdomain/zero/1.0/deed.en + */ + +#ifndef LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ +#define LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ + +#include + +#include +#include +#include +#include + +#define CHECK_CUDA_ERROR(e) (cuda::check_error(e, __FILE__, __LINE__)) + +namespace cuda +{ +inline void check_error(const ::cudaError_t e, const char * f, int n) +{ + if (e != ::cudaSuccess) { + ::std::stringstream s; + s << ::cudaGetErrorName(e) << " (" << e << ")@" << f << "#L" << n << ": " + << ::cudaGetErrorString(e); + throw ::std::runtime_error{s.str()}; + } +} + +struct deleter +{ + void operator()(void * p) const { CHECK_CUDA_ERROR(::cudaFree(p)); } +}; + +template +using unique_ptr = ::std::unique_ptr; + +template +typename ::std::enable_if<::std::is_array::value, cuda::unique_ptr>::type make_unique( + const ::std::size_t n) +{ + using U = typename ::std::remove_extent::type; + U * p; + CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(U) * n)); + return cuda::unique_ptr{p}; +} + +template +cuda::unique_ptr make_unique() +{ + T * p; + CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(T))); + return cuda::unique_ptr{p}; +} + +constexpr size_t CUDA_ALIGN = 256; + +template +inline size_t get_size_aligned(size_t num_elem) +{ + size_t size = num_elem * sizeof(T); + size_t extra_align = 0; + if (size % CUDA_ALIGN != 0) { + extra_align = CUDA_ALIGN - size % CUDA_ALIGN; + } + return size + extra_align; +} + +template +inline T * get_next_ptr(size_t num_elem, void *& workspace, size_t & workspace_size) +{ + size_t size = get_size_aligned(num_elem); + if (size > workspace_size) { + throw ::std::runtime_error("Workspace is too small!"); + } + workspace_size -= size; + T * ptr = reinterpret_cast(workspace); + workspace = reinterpret_cast(reinterpret_cast(workspace) + size); + return ptr; +} + +template +void clear_async(T * ptr, size_t num_elem, cudaStream_t stream) +{ + CHECK_CUDA_ERROR(::cudaMemsetAsync(ptr, 0, sizeof(T) * num_elem, stream)); +} + +} // namespace cuda + +#endif // LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp b/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp new file mode 100644 index 0000000000000..ace7dc5ff3405 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp @@ -0,0 +1,47 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ +#define LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ + +#include + +#include +#include +#include +#include + +#include + +namespace lidar_transfusion +{ + +class DetectionClassRemapper +{ +public: + void setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix); + void mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg); + +protected: + Eigen::Matrix allow_remapping_by_area_matrix_; + Eigen::MatrixXd min_area_matrix_; + Eigen::MatrixXd max_area_matrix_; + int num_labels_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp new file mode 100644 index 0000000000000..86303618d7877 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp @@ -0,0 +1,73 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ +#define LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ + +#include "lidar_transfusion/detection_class_remapper.hpp" +#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "lidar_transfusion/transfusion_trt.hpp" +#include "lidar_transfusion/visibility_control.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace lidar_transfusion +{ + +class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node +{ +public: + explicit LidarTransfusionNode(const rclcpp::NodeOptions & options); + +private: + void cloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + + rclcpp::Subscription::ConstSharedPtr cloud_sub_{nullptr}; + rclcpp::Publisher::SharedPtr objects_pub_{ + nullptr}; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + + DetectionClassRemapper detection_class_remapper_; + float score_threshold_{0.0}; + std::vector class_names_; + + NonMaximumSuppression iou_bev_nms_; + + std::unique_ptr detector_ptr_{nullptr}; + + // debugger + std::unique_ptr> stop_watch_ptr_{ + nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr published_time_pub_{nullptr}; +}; +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp b/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp new file mode 100644 index 0000000000000..3f8f32e346b0a --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp @@ -0,0 +1,87 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ +#define LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ + +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +struct ProfileDimension +{ + nvinfer1::Dims min; + nvinfer1::Dims opt; + nvinfer1::Dims max; + + bool operator!=(const ProfileDimension & rhs) const + { + return min.nbDims != rhs.min.nbDims || opt.nbDims != rhs.opt.nbDims || + max.nbDims != rhs.max.nbDims || !std::equal(min.d, min.d + min.nbDims, rhs.min.d) || + !std::equal(opt.d, opt.d + opt.nbDims, rhs.opt.d) || + !std::equal(max.d, max.d + max.nbDims, rhs.max.d); + } +}; + +class NetworkTRT +{ +public: + explicit NetworkTRT(const TransfusionConfig & config); + ~NetworkTRT(); + + bool init( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision); + const char * getTensorName(NetworkIO name); + + tensorrt_common::TrtUniquePtr engine{nullptr}; + tensorrt_common::TrtUniquePtr context{nullptr}; + +private: + bool parseONNX( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision, + size_t workspace_size = (1ULL << 30)); + bool saveEngine(const std::string & engine_path); + bool loadEngine(const std::string & engine_path); + bool createContext(); + bool setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config); + bool validateNetworkIO(); + nvinfer1::Dims validateTensorShape(NetworkIO name, const std::vector shape); + + tensorrt_common::TrtUniquePtr runtime_{nullptr}; + tensorrt_common::TrtUniquePtr plan_{nullptr}; + tensorrt_common::Logger logger_; + TransfusionConfig config_; + std::vector tensors_names_; + + std::array in_profile_dims_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp new file mode 100644 index 0000000000000..e231482652b98 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp @@ -0,0 +1,32 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#define LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ + +#include "lidar_transfusion/utils.hpp" + +#include + +namespace lidar_transfusion +{ +// Non-maximum suppression (NMS) uses the distance on the xy plane instead of +// intersection over union (IoU) to suppress overlapped objects. +std::size_t circleNMS( + thrust::device_vector & boxes3d, const float distance_threshold, + thrust::device_vector & keep_mask, cudaStream_t stream); + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp new file mode 100644 index 0000000000000..daf2cc7c1fac1 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp @@ -0,0 +1,80 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#define LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ + +#include "lidar_transfusion/ros_utils.hpp" + +#include + +#include + +#include +#include + +namespace lidar_transfusion +{ +using autoware_perception_msgs::msg::DetectedObject; + +enum class NMS_TYPE { + IoU_BEV + // IoU_3D + // Distance_2D + // Distance_3D +}; + +struct NMSParams +{ + NMS_TYPE nms_type_{}; + std::vector target_class_names_{}; + double search_distance_2d_{}; + double iou_threshold_{}; + // double distance_threshold_{}; +}; + +std::vector classNamesToBooleanMask(const std::vector & class_names) +{ + std::vector mask; + constexpr std::size_t num_object_classification = 8; + mask.resize(num_object_classification); + for (const auto & class_name : class_names) { + const auto semantic_type = getSemanticType(class_name); + mask.at(semantic_type) = true; + } + + return mask; +} + +class NonMaximumSuppression +{ +public: + void setParameters(const NMSParams &); + + std::vector apply(const std::vector &); + +private: + bool isTargetLabel(const std::uint8_t); + + bool isTargetPairObject(const DetectedObject &, const DetectedObject &); + + Eigen::MatrixXd generateIoUMatrix(const std::vector &); + + NMSParams params_{}; + std::vector target_class_mask_{}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp new file mode 100644 index 0000000000000..01435424aa248 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#define LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ + +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include +#include +#include + +#include + +namespace lidar_transfusion +{ + +class PostprocessCuda +{ +public: + explicit PostprocessCuda(const TransfusionConfig & config, cudaStream_t & stream); + + cudaError_t generateDetectedBoxes3D_launch( + const float * cls_output, const float * box_output, const float * dir_cls_output, + std::vector & det_boxes3d, cudaStream_t stream); + +private: + TransfusionConfig config_; + cudaStream_t stream_; + cudaStream_t stream_event_; + cudaEvent_t start_, stop_; + thrust::device_vector boxes3d_d_; + thrust::device_vector yaw_norm_thresholds_d_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp new file mode 100644 index 0000000000000..25095f4dc9c0b --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp @@ -0,0 +1,101 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#define LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" + +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include + +namespace lidar_transfusion +{ + +class DensificationParam +{ +public: + DensificationParam(const std::string & world_frame_id, const unsigned int num_past_frames) + : world_frame_id_(std::move(world_frame_id)), + pointcloud_cache_size_(num_past_frames + /*current frame*/ 1) + { + } + + std::string world_frame_id() const { return world_frame_id_; } + unsigned int pointcloud_cache_size() const { return pointcloud_cache_size_; } + +private: + std::string world_frame_id_; + unsigned int pointcloud_cache_size_{1}; +}; + +struct PointCloudWithTransform +{ + cuda::unique_ptr points_d{nullptr}; + std_msgs::msg::Header header; + size_t num_points{0}; + Eigen::Affine3f affine_past2world; +}; + +class PointCloudDensification +{ +public: + explicit PointCloudDensification(const DensificationParam & param, cudaStream_t & stream); + + bool enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); + + double getCurrentTimestamp() const { return current_timestamp_; } + Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; } + std::list::iterator getPointCloudCacheIter() + { + return pointcloud_cache_.begin(); + } + bool isCacheEnd(std::list::iterator iter) + { + return iter == pointcloud_cache_.end(); + } + size_t getIdx(std::list::iterator iter) + { + return std::distance(pointcloud_cache_.begin(), iter); + } + size_t getCacheSize() + { + return std::distance(pointcloud_cache_.begin(), pointcloud_cache_.end()); + } + unsigned int pointcloud_cache_size() const { return param_.pointcloud_cache_size(); } + +private: + void enqueue(const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine); + void dequeue(); + + DensificationParam param_; + double current_timestamp_{0.0}; + Eigen::Affine3f affine_world2current_; + std::list pointcloud_cache_; + cudaStream_t stream_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp new file mode 100644 index 0000000000000..c571990d84b51 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 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. +/* + * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#define LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +namespace lidar_transfusion +{ + +class PreprocessCuda +{ +public: + PreprocessCuda(const TransfusionConfig & config, cudaStream_t & stream); + + void generateVoxels( + float * points, unsigned int points_size, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs); + + cudaError_t generateVoxels_random_launch( + float * points, unsigned int points_size, unsigned int * mask, float * voxels); + + cudaError_t generateBaseFeatures_launch( + unsigned int * mask, float * voxels, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs); + + // cudaError_t generateVoxelsInput_launch( + // uint8_t * cloud_data, CloudInfo & cloud_info, unsigned int points_agg, unsigned int + // points_size, float time_lag, float * affine_transform, float * points); + + cudaError_t generateSweepPoints_launch( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform, float * output_points); + +private: + TransfusionConfig config_; + cudaStream_t stream_; + cuda::unique_ptr mask_{nullptr}; + cuda::unique_ptr voxels_{nullptr}; + unsigned int mask_size_; + unsigned int voxels_size_; +}; +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp new file mode 100644 index 0000000000000..3e3660e238473 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp @@ -0,0 +1,70 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ +#define LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include +#include +#include + +namespace lidar_transfusion +{ +constexpr size_t AFF_MAT_SIZE = 16; // 4x4 matrix +constexpr size_t MAX_CLOUD_STEP_SIZE = 35; // PointXYZIRCADT + +class VoxelGenerator +{ +public: + explicit VoxelGenerator( + const DensificationParam & densification_param, const TransfusionConfig & config, + cudaStream_t & stream); + std::size_t generateSweepPoints( + const sensor_msgs::msg::PointCloud2 & msg, cuda::unique_ptr & points_d); + bool enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); + void initCloudInfo(const sensor_msgs::msg::PointCloud2 & msg); + std::tuple getFieldInfo( + const sensor_msgs::msg::PointCloud2 & msg, const std::string & field_name); + +private: + std::unique_ptr pd_ptr_{nullptr}; + std::unique_ptr pre_ptr_{nullptr}; + TransfusionConfig config_; + cuda::unique_ptr cloud_data_d_{nullptr}; + cuda::unique_ptr affine_past2current_d_{nullptr}; + std::vector points_; + cudaStream_t stream_; + CloudInfo cloud_info_; + bool is_initialized_{false}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp new file mode 100644 index 0000000000000..f013a02404a29 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp @@ -0,0 +1,39 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__ROS_UTILS_HPP_ +#define LIDAR_TRANSFUSION__ROS_UTILS_HPP_ + +#include "lidar_transfusion/utils.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace lidar_transfusion +{ + +void box3DToDetectedObject( + const Box3D & box3d, const std::vector & class_names, + autoware_perception_msgs::msg::DetectedObject & obj); + +uint8_t getSemanticType(const std::string & class_name); + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__ROS_UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp new file mode 100644 index 0000000000000..31976de56a9da --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp @@ -0,0 +1,161 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ +#define LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ + +#include +#include + +namespace lidar_transfusion +{ + +class TransfusionConfig +{ +public: + TransfusionConfig( + const std::vector & voxels_num, const std::vector & point_cloud_range, + const std::vector & voxel_size, const float circle_nms_dist_threshold, + const std::vector & yaw_norm_thresholds, const float score_threshold) + { + if (voxels_num.size() == 3) { + max_voxels_ = voxels_num[2]; + + voxels_num_[0] = voxels_num[0]; + voxels_num_[1] = voxels_num[1]; + voxels_num_[2] = voxels_num[2]; + + min_voxel_size_ = voxels_num[0]; + opt_voxel_size_ = voxels_num[1]; + max_voxel_size_ = voxels_num[2]; + + min_points_size_ = voxels_num[0]; + opt_points_size_ = voxels_num[1]; + max_points_size_ = voxels_num[2]; + + min_coors_size_ = voxels_num[0]; + opt_coors_size_ = voxels_num[1]; + max_coors_size_ = voxels_num[2]; + } + if (point_cloud_range.size() == 6) { + min_x_range_ = static_cast(point_cloud_range[0]); + min_y_range_ = static_cast(point_cloud_range[1]); + min_z_range_ = static_cast(point_cloud_range[2]); + max_x_range_ = static_cast(point_cloud_range[3]); + max_y_range_ = static_cast(point_cloud_range[4]); + max_z_range_ = static_cast(point_cloud_range[5]); + } + if (voxel_size.size() == 3) { + voxel_x_size_ = static_cast(voxel_size[0]); + voxel_y_size_ = static_cast(voxel_size[1]); + voxel_z_size_ = static_cast(voxel_size[2]); + } + if (score_threshold > 0.0) { + score_threshold_ = score_threshold; + } + if (circle_nms_dist_threshold > 0.0) { + circle_nms_dist_threshold_ = circle_nms_dist_threshold; + } + yaw_norm_thresholds_ = + std::vector(yaw_norm_thresholds.begin(), yaw_norm_thresholds.end()); + for (auto & yaw_norm_threshold : yaw_norm_thresholds_) { + yaw_norm_threshold = + (yaw_norm_threshold >= 0.0 && yaw_norm_threshold < 1.0) ? yaw_norm_threshold : 0.0; + } + grid_x_size_ = static_cast((max_x_range_ - min_x_range_) / voxel_x_size_); + grid_y_size_ = static_cast((max_y_range_ - min_y_range_) / voxel_y_size_); + grid_z_size_ = static_cast((max_z_range_ - min_z_range_) / voxel_z_size_); + } + + ///// INPUT PARAMETERS ///// + const std::size_t cloud_capacity_{1000000}; + ///// KERNEL PARAMETERS ///// + const std::size_t threads_for_voxel_{256}; // threads number for a block + const std::size_t points_per_voxel_{20}; + const std::size_t warp_size_{32}; // one warp(32 threads) for one pillar + const std::size_t pillars_per_block_{64}; // one thread deals with one pillar + // and a block has pillars_per_block threads + const std::size_t pillar_feature_size_{64}; + std::size_t max_voxels_{60000}; + + ///// NETWORK PARAMETERS ///// + const std::size_t batch_size_{1}; + const std::size_t num_classes_{5}; + const std::size_t num_point_feature_size_{5}; // x, y, z, intensity, lag + // the dimension of the input cloud + float min_x_range_{-76.8}; + float max_x_range_{76.8}; + float min_y_range_{-76.8}; + float max_y_range_{76.8}; + float min_z_range_{-3.0}; + float max_z_range_{5.0}; + // the size of a pillar + float voxel_x_size_{0.3}; + float voxel_y_size_{0.3}; + float voxel_z_size_{8.0}; + const std::size_t out_size_factor_{4}; + const std::size_t max_num_points_per_pillar_{points_per_voxel_}; + const std::size_t num_point_values_{4}; + const std::size_t num_proposals_{200}; + // the number of feature maps for pillar scatter + const std::size_t num_feature_scatter_{pillar_feature_size_}; + // the score threshold for classification + float score_threshold_{0.2}; + float circle_nms_dist_threshold_{0.5}; + std::vector yaw_norm_thresholds_{0.3, 0.3, 0.3, 0.3, 0.0}; + std::size_t max_num_pillars_{max_voxels_}; + const std::size_t pillar_points_bev_{max_num_points_per_pillar_ * max_num_pillars_}; + // the detected boxes result decode by (x, y, z, w, l, h, yaw) + const std::size_t num_box_values_{8}; + // the input size of the 2D backbone network + std::size_t grid_x_size_{512}; + std::size_t grid_y_size_{512}; + std::size_t grid_z_size_{1}; + // the output size of the 2D backbone network + std::size_t feature_x_size_{grid_x_size_ / out_size_factor_}; + std::size_t feature_y_size_{grid_y_size_ / out_size_factor_}; + + ///// RUNTIME DIMENSIONS ///// + std::vector voxels_num_{5000, 30000, 60000}; + // voxels + std::size_t min_voxel_size_{voxels_num_[0]}; + std::size_t opt_voxel_size_{voxels_num_[1]}; + std::size_t max_voxel_size_{voxels_num_[2]}; + + std::size_t min_point_in_voxel_size_{points_per_voxel_}; + std::size_t opt_point_in_voxel_size_{points_per_voxel_}; + std::size_t max_point_in_voxel_size_{points_per_voxel_}; + + std::size_t min_network_feature_size_{num_point_feature_size_}; + std::size_t opt_network_feature_size_{num_point_feature_size_}; + std::size_t max_network_feature_size_{num_point_feature_size_}; + + // num_points + std::size_t min_points_size_{voxels_num_[0]}; + std::size_t opt_points_size_{voxels_num_[1]}; + std::size_t max_points_size_{voxels_num_[2]}; + + // coors + std::size_t min_coors_size_{voxels_num_[0]}; + std::size_t opt_coors_size_{voxels_num_[1]}; + std::size_t max_coors_size_{voxels_num_[2]}; + + std::size_t min_coors_dim_size_{num_point_values_}; + std::size_t opt_coors_dim_size_{num_point_values_}; + std::size_t max_coors_dim_size_{num_point_values_}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp new file mode 100644 index 0000000000000..a94c3c1871136 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp @@ -0,0 +1,114 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ +#define LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/network/network_trt.hpp" +#include "lidar_transfusion/postprocess/postprocess_kernel.hpp" +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/preprocess/voxel_generator.hpp" +#include "lidar_transfusion/utils.hpp" +#include "lidar_transfusion/visibility_control.hpp" + +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +class NetworkParam +{ +public: + NetworkParam(std::string onnx_path, std::string engine_path, std::string trt_precision) + : onnx_path_(std::move(onnx_path)), + engine_path_(std::move(engine_path)), + trt_precision_(std::move(trt_precision)) + { + } + + std::string onnx_path() const { return onnx_path_; } + std::string engine_path() const { return engine_path_; } + std::string trt_precision() const { return trt_precision_; } + +private: + std::string onnx_path_; + std::string engine_path_; + std::string trt_precision_; +}; + +class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT +{ +public: + explicit TransfusionTRT( + const NetworkParam & network_param, const DensificationParam & densification_param, + const TransfusionConfig & config); + virtual ~TransfusionTRT(); + + bool detect( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer, + std::vector & det_boxes3d, std::unordered_map & proc_timing); + +protected: + void initPtr(); + + bool preprocess(const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); + + bool inference(); + + bool postprocess(std::vector & det_boxes3d); + + std::unique_ptr network_trt_ptr_{nullptr}; + std::unique_ptr vg_ptr_{nullptr}; + std::unique_ptr> stop_watch_ptr_{ + nullptr}; + std::unique_ptr pre_ptr_{nullptr}; + std::unique_ptr post_ptr_{nullptr}; + cudaStream_t stream_{nullptr}; + + TransfusionConfig config_; + + // input of pre-process + + unsigned int voxel_features_size_{0}; + unsigned int voxel_num_size_{0}; + unsigned int voxel_idxs_size_{0}; + unsigned int cls_size_{0}; + unsigned int box_size_{0}; + unsigned int dir_cls_size_{0}; + cuda::unique_ptr points_d_{nullptr}; + cuda::unique_ptr params_input_d_{nullptr}; + cuda::unique_ptr voxel_features_d_{nullptr}; + cuda::unique_ptr voxel_num_d_{nullptr}; + cuda::unique_ptr voxel_idxs_d_{nullptr}; + cuda::unique_ptr cls_output_d_{nullptr}; + cuda::unique_ptr box_output_d_{nullptr}; + cuda::unique_ptr dir_cls_output_d_{nullptr}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp new file mode 100644 index 0000000000000..e73fe7f055953 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp @@ -0,0 +1,103 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__UTILS_HPP_ +#define LIDAR_TRANSFUSION__UTILS_HPP_ + +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +struct Box3D +{ + int label; + float score; + float x; + float y; + float z; + float width; + float length; + float height; + float yaw; +}; + +struct CloudInfo +{ + uint32_t x_offset; + uint32_t y_offset; + uint32_t z_offset; + uint32_t intensity_offset; + uint8_t x_datatype; + uint8_t y_datatype; + uint8_t z_datatype; + uint8_t intensity_datatype; + uint8_t x_count; + uint8_t y_count; + uint8_t z_count; + uint8_t intensity_count; + uint32_t point_step; + bool is_bigendian; + + CloudInfo() + : x_offset(0), + y_offset(4), + z_offset(8), + intensity_offset(12), + x_datatype(7), + y_datatype(7), + z_datatype(7), + intensity_datatype(7), + x_count(1), + y_count(1), + z_count(1), + intensity_count(1), + point_step(16), + is_bigendian(false) + { + } + + bool operator!=(const CloudInfo & rhs) const + { + return x_offset != rhs.x_offset || y_offset != rhs.y_offset || z_offset != rhs.z_offset || + intensity_offset != rhs.intensity_offset || x_datatype != rhs.x_datatype || + y_datatype != rhs.y_datatype || z_datatype != rhs.z_datatype || + intensity_datatype != rhs.intensity_datatype || x_count != rhs.x_count || + y_count != rhs.y_count || z_count != rhs.z_count || + intensity_count != rhs.intensity_count || is_bigendian != rhs.is_bigendian; + } +}; + +enum NetworkIO { voxels = 0, num_points, coors, cls_score, dir_pred, bbox_pred, ENUM_SIZE }; + +// cspell: ignore divup +template +unsigned int divup(const T1 a, const T2 b) +{ + if (a == 0) { + throw std::runtime_error("A dividend of divup isn't positive."); + } + if (b == 0) { + throw std::runtime_error("A divisor of divup isn't positive."); + } + + return (a + b - 1) / b; +} + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp b/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp new file mode 100644 index 0000000000000..aeab20906628a --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ +#define LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) +#if defined(LIDAR_TRANSFUSION_BUILDING_DLL) || defined(LIDAR_TRANSFUSION_EXPORTS) +#define LIDAR_TRANSFUSION_PUBLIC __declspec(dllexport) +#define LIDAR_TRANSFUSION_LOCAL +#else // defined(LIDAR_TRANSFUSION_BUILDING_DLL) || defined(LIDAR_TRANSFUSION_EXPORTS) +#define LIDAR_TRANSFUSION_PUBLIC __declspec(dllimport) +#define LIDAR_TRANSFUSION_LOCAL +#endif // defined(LIDAR_TRANSFUSION_BUILDING_DLL) || defined(LIDAR_TRANSFUSION_EXPORTS) +#elif defined(__linux__) +#define LIDAR_TRANSFUSION_PUBLIC __attribute__((visibility("default"))) +#define LIDAR_TRANSFUSION_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define LIDAR_TRANSFUSION_PUBLIC __attribute__((visibility("default"))) +#define LIDAR_TRANSFUSION_LOCAL __attribute__((visibility("hidden"))) +#else +#error "Unsupported Build Configuration" +#endif + +#endif // LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml b/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml new file mode 100644 index 0000000000000..492eb8c4d59b7 --- /dev/null +++ b/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/lidar_transfusion/lib/detection_class_remapper.cpp b/perception/lidar_transfusion/lib/detection_class_remapper.cpp new file mode 100644 index 0000000000000..e093637402448 --- /dev/null +++ b/perception/lidar_transfusion/lib/detection_class_remapper.cpp @@ -0,0 +1,71 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace lidar_transfusion +{ + +void DetectionClassRemapper::setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix) +{ + assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size()); + assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size()); + assert( + std::pow(static_cast(std::sqrt(min_area_matrix.size())), 2) == min_area_matrix.size()); + + num_labels_ = static_cast(std::sqrt(min_area_matrix.size())); + + Eigen::Map> + allow_remapping_by_area_matrix_tmp( + allow_remapping_by_area_matrix.data(), num_labels_, num_labels_); + allow_remapping_by_area_matrix_ = allow_remapping_by_area_matrix_tmp.transpose() + .cast(); // Eigen is column major by default + + Eigen::Map min_area_matrix_tmp( + min_area_matrix.data(), num_labels_, num_labels_); + min_area_matrix_ = min_area_matrix_tmp.transpose(); // Eigen is column major by default + + Eigen::Map max_area_matrix_tmp( + max_area_matrix.data(), num_labels_, num_labels_); + max_area_matrix_ = max_area_matrix_tmp.transpose(); // Eigen is column major by default + + min_area_matrix_ = min_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); + max_area_matrix_ = max_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); +} + +void DetectionClassRemapper::mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg) +{ + for (auto & object : msg.objects) { + const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y; + + for (auto & classification : object.classification) { + auto & label = classification.label; + + for (int i = 0; i < num_labels_; ++i) { + if ( + allow_remapping_by_area_matrix_(label, i) && bev_area >= min_area_matrix_(label, i) && + bev_area <= max_area_matrix_(label, i)) { + label = i; + break; + } + } + } + } +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/network/network_trt.cpp b/perception/lidar_transfusion/lib/network/network_trt.cpp new file mode 100644 index 0000000000000..a36890facceec --- /dev/null +++ b/perception/lidar_transfusion/lib/network/network_trt.cpp @@ -0,0 +1,332 @@ +// Copyright 2024 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 "lidar_transfusion/network/network_trt.hpp" + +#include + +#include +#include +#include + +namespace lidar_transfusion +{ + +std::ostream & operator<<(std::ostream & os, const ProfileDimension & profile) +{ + std::string delim = ""; + os << "min->["; + for (int i = 0; i < profile.min.nbDims; ++i) { + os << delim << profile.min.d[i]; + delim = ", "; + } + os << "], opt->["; + delim = ""; + for (int i = 0; i < profile.opt.nbDims; ++i) { + os << delim << profile.opt.d[i]; + delim = ", "; + } + os << "], max->["; + delim = ""; + for (int i = 0; i < profile.max.nbDims; ++i) { + os << delim << profile.max.d[i]; + delim = ", "; + } + os << "]"; + return os; +} + +NetworkTRT::NetworkTRT(const TransfusionConfig & config) : config_(config) +{ + ProfileDimension voxels_dims = { + nvinfer1::Dims3( + config_.min_voxel_size_, config_.min_point_in_voxel_size_, config_.min_network_feature_size_), + nvinfer1::Dims3( + config_.opt_voxel_size_, config_.opt_point_in_voxel_size_, config_.opt_network_feature_size_), + nvinfer1::Dims3( + config_.max_voxel_size_, config_.max_point_in_voxel_size_, + config_.max_network_feature_size_)}; + ProfileDimension num_points_dims = { + nvinfer1::Dims{1, {static_cast(config_.min_points_size_)}}, + nvinfer1::Dims{1, {static_cast(config_.opt_points_size_)}}, + nvinfer1::Dims{1, {static_cast(config_.max_points_size_)}}}; + ProfileDimension coors_dims = { + nvinfer1::Dims2(config_.min_coors_size_, config_.min_coors_dim_size_), + nvinfer1::Dims2(config_.opt_coors_size_, config_.opt_coors_dim_size_), + nvinfer1::Dims2(config_.max_coors_size_, config_.max_coors_dim_size_)}; + in_profile_dims_ = {voxels_dims, num_points_dims, coors_dims}; +} + +NetworkTRT::~NetworkTRT() +{ + context.reset(); + runtime_.reset(); + plan_.reset(); + engine.reset(); +} + +bool NetworkTRT::init( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision) +{ + runtime_ = + tensorrt_common::TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); + if (!runtime_) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create runtime" << std::endl; + return false; + } + + bool success; + std::ifstream engine_file(engine_path); + if (engine_file.is_open()) { + success = loadEngine(engine_path); + } else { + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait a minutes...", 5); + success = parseONNX(onnx_path, engine_path, precision); + logger_.stop_throttle(log_thread); + } + success &= createContext(); + + return success; +} + +bool NetworkTRT::setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config) +{ + auto profile = builder.createOptimizationProfile(); + + auto voxels_name = network.getInput(NetworkIO::voxels)->getName(); + auto num_points_name = network.getInput(NetworkIO::num_points)->getName(); + auto coors_name = network.getInput(NetworkIO::coors)->getName(); + + profile->setDimensions( + voxels_name, nvinfer1::OptProfileSelector::kMIN, in_profile_dims_[NetworkIO::voxels].min); + profile->setDimensions( + voxels_name, nvinfer1::OptProfileSelector::kOPT, in_profile_dims_[NetworkIO::voxels].opt); + profile->setDimensions( + voxels_name, nvinfer1::OptProfileSelector::kMAX, in_profile_dims_[NetworkIO::voxels].max); + + profile->setDimensions( + num_points_name, nvinfer1::OptProfileSelector::kMIN, + in_profile_dims_[NetworkIO::num_points].min); + profile->setDimensions( + num_points_name, nvinfer1::OptProfileSelector::kOPT, + in_profile_dims_[NetworkIO::num_points].opt); + profile->setDimensions( + num_points_name, nvinfer1::OptProfileSelector::kMAX, + in_profile_dims_[NetworkIO::num_points].max); + + profile->setDimensions( + coors_name, nvinfer1::OptProfileSelector::kMIN, in_profile_dims_[NetworkIO::coors].min); + profile->setDimensions( + coors_name, nvinfer1::OptProfileSelector::kOPT, in_profile_dims_[NetworkIO::coors].opt); + profile->setDimensions( + coors_name, nvinfer1::OptProfileSelector::kMAX, in_profile_dims_[NetworkIO::coors].max); + + config.addOptimizationProfile(profile); + return true; +} + +bool NetworkTRT::createContext() +{ + if (!engine) { + tensorrt_common::LOG_ERROR(logger_) + << "Failed to create context: Engine was not created" << std::endl; + return false; + } + + context = + tensorrt_common::TrtUniquePtr(engine->createExecutionContext()); + if (!context) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; + return false; + } + + return true; +} + +bool NetworkTRT::parseONNX( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision, + const size_t workspace_size) +{ + auto builder = + tensorrt_common::TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); + if (!builder) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; + return false; + } + + auto config = + tensorrt_common::TrtUniquePtr(builder->createBuilderConfig()); + if (!config) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create config" << std::endl; + return false; + } +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 + config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); +#else + config->setMaxWorkspaceSize(workspace_size); +#endif + if (precision == "fp16") { + if (builder->platformHasFastFp16()) { + tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; + config->setFlag(nvinfer1::BuilderFlag::kFP16); + } else { + tensorrt_common::LOG_INFO(logger_) + << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; + } + } + + const auto flag = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + auto network = + tensorrt_common::TrtUniquePtr(builder->createNetworkV2(flag)); + if (!network) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; + return false; + } + + auto parser = tensorrt_common::TrtUniquePtr( + nvonnxparser::createParser(*network, logger_)); + parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); + + if (!setProfile(*builder, *network, *config)) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; + return false; + } + + plan_ = tensorrt_common::TrtUniquePtr( + builder->buildSerializedNetwork(*network, *config)); + if (!plan_) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create serialized network" << std::endl; + return false; + } + engine = tensorrt_common::TrtUniquePtr( + runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); + if (!engine) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; + return false; + } + + return saveEngine(engine_path); +} + +bool NetworkTRT::saveEngine(const std::string & engine_path) +{ + tensorrt_common::LOG_INFO(logger_) << "Writing to " << engine_path << std::endl; + std::ofstream file(engine_path, std::ios::out | std::ios::binary); + file.write(reinterpret_cast(plan_->data()), plan_->size()); + return validateNetworkIO(); +} + +bool NetworkTRT::loadEngine(const std::string & engine_path) +{ + std::ifstream engine_file(engine_path); + std::stringstream engine_buffer; + engine_buffer << engine_file.rdbuf(); + std::string engine_str = engine_buffer.str(); + engine = tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( + reinterpret_cast(engine_str.data()), engine_str.size())); + tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; + return validateNetworkIO(); +} + +bool NetworkTRT::validateNetworkIO() +{ + // Whether the number of IO match the expected size + if (engine->getNbIOTensors() != NetworkIO::ENUM_SIZE) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid network IO. Expected size: " << NetworkIO::ENUM_SIZE + << ". Actual size: " << engine->getNbIOTensors() << "." << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + for (int i = 0; i < NetworkIO::ENUM_SIZE; ++i) { + tensors_names_.push_back(engine->getIOTensorName(i)); + } + + // Log the network IO + std::string tensors = std::accumulate( + tensors_names_.begin(), tensors_names_.end(), std::string(), + [](const std::string & a, const std::string & b) -> std::string { return a + b + " "; }); + tensorrt_common::LOG_INFO(logger_) << "Network IO: " << tensors << std::endl; + + // Whether the current engine input profile match the config input profile + for (int i = 0; i <= NetworkIO::coors; ++i) { + ProfileDimension engine_dims{ + engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kMIN), + engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kOPT), + engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kMAX)}; + + tensorrt_common::LOG_INFO(logger_) + << "Profile for " << tensors_names_[i] << ": " << engine_dims << std::endl; + + if (engine_dims != in_profile_dims_[i]) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid network input dimension. Config: " << in_profile_dims_[i] + << ". Please change the input profile or delete the engine file and build engine again." + << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + } + + // Whether the IO tensor shapes match the network config, -1 for dynamic size + validateTensorShape( + NetworkIO::voxels, {-1, static_cast(config_.points_per_voxel_), + static_cast(config_.num_point_feature_size_)}); + validateTensorShape(NetworkIO::num_points, {-1}); + validateTensorShape(NetworkIO::coors, {-1, static_cast(config_.num_point_values_)}); + auto cls_score = validateTensorShape( + NetworkIO::cls_score, + {static_cast(config_.batch_size_), static_cast(config_.num_classes_), + static_cast(config_.num_proposals_)}); + tensorrt_common::LOG_INFO(logger_) << "Network num classes: " << cls_score.d[1] << std::endl; + validateTensorShape( + NetworkIO::dir_pred, + {static_cast(config_.batch_size_), 2, static_cast(config_.num_proposals_)}); // x, y + validateTensorShape( + NetworkIO::bbox_pred, + {static_cast(config_.batch_size_), static_cast(config_.num_box_values_), + static_cast(config_.num_proposals_)}); + + return true; +} + +const char * NetworkTRT::getTensorName(NetworkIO name) +{ + return tensors_names_.at(name); +} + +nvinfer1::Dims NetworkTRT::validateTensorShape(NetworkIO name, const std::vector shape) +{ + auto tensor_shape = engine->getTensorShape(tensors_names_[name]); + if (tensor_shape.nbDims != static_cast(shape.size())) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid tensor shape for " << tensors_names_[name] << ". Expected size: " << shape.size() + << ". Actual size: " << tensor_shape.nbDims << "." << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + for (int i = 0; i < tensor_shape.nbDims; ++i) { + if (tensor_shape.d[i] != static_cast(shape[i])) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid tensor shape for " << tensors_names_[name] << ". Expected: " << shape[i] + << ". Actual: " << tensor_shape.d[i] << "." << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + } + return tensor_shape; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu b/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu new file mode 100644 index 0000000000000..dcb60831825bb --- /dev/null +++ b/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu @@ -0,0 +1,144 @@ +// Copyright 2024 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. + +// Modified from +// https://github.com/open-mmlab/OpenPCDet/blob/master/pcdet/ops/iou3d_nms/src/iou3d_nms_kernel.cu + +/* +3D IoU Calculation and Rotated NMS(modified from 2D NMS written by others) +Written by Shaoshuai Shi +All Rights Reserved 2019-2020. +*/ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/postprocess/circle_nms_kernel.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +namespace +{ +const std::size_t THREADS_PER_BLOCK_NMS = 16; +} // namespace + +namespace lidar_transfusion +{ + +__device__ inline float dist2dPow(const Box3D * a, const Box3D * b) +{ + return powf(a->x - b->x, 2) + powf(a->y - b->y, 2); +} + +// cspell: ignore divup +__global__ void circleNMS_Kernel( + const Box3D * boxes, const std::size_t num_boxes3d, const std::size_t col_blocks, + const float dist2d_pow_threshold, std::uint64_t * mask) +{ + // params: boxes (N,) + // params: mask (N, divup(N/THREADS_PER_BLOCK_NMS)) + + const auto row_start = blockIdx.y; + const auto col_start = blockIdx.x; + + if (row_start > col_start) return; + + const std::size_t row_size = + fminf(num_boxes3d - row_start * THREADS_PER_BLOCK_NMS, THREADS_PER_BLOCK_NMS); + const std::size_t col_size = + fminf(num_boxes3d - col_start * THREADS_PER_BLOCK_NMS, THREADS_PER_BLOCK_NMS); + + __shared__ Box3D block_boxes[THREADS_PER_BLOCK_NMS]; + + if (threadIdx.x < col_size) { + block_boxes[threadIdx.x] = boxes[THREADS_PER_BLOCK_NMS * col_start + threadIdx.x]; + } + __syncthreads(); + + if (threadIdx.x < row_size) { + const std::size_t cur_box_idx = THREADS_PER_BLOCK_NMS * row_start + threadIdx.x; + const Box3D * cur_box = boxes + cur_box_idx; + + std::uint64_t t = 0; + std::size_t start = 0; + if (row_start == col_start) { + start = threadIdx.x + 1; + } + for (std::size_t i = start; i < col_size; i++) { + if (dist2dPow(cur_box, block_boxes + i) < dist2d_pow_threshold) { + t |= 1ULL << i; + } + } + mask[cur_box_idx * col_blocks + col_start] = t; + } +} + +cudaError_t circleNMS_launch( + const thrust::device_vector & boxes3d, const std::size_t num_boxes3d, + std::size_t col_blocks, const float distance_threshold, + thrust::device_vector & mask, cudaStream_t stream) +{ + const float dist2d_pow_thres = powf(distance_threshold, 2); + + dim3 blocks(col_blocks, col_blocks); + dim3 threads(THREADS_PER_BLOCK_NMS); + circleNMS_Kernel<<>>( + thrust::raw_pointer_cast(boxes3d.data()), num_boxes3d, col_blocks, dist2d_pow_thres, + thrust::raw_pointer_cast(mask.data())); + + return cudaGetLastError(); +} + +std::size_t circleNMS( + thrust::device_vector & boxes3d, const float distance_threshold, + thrust::device_vector & keep_mask, cudaStream_t stream) +{ + const auto num_boxes3d = boxes3d.size(); + const auto col_blocks = divup(num_boxes3d, THREADS_PER_BLOCK_NMS); + thrust::device_vector mask_d(num_boxes3d * col_blocks); + + CHECK_CUDA_ERROR( + circleNMS_launch(boxes3d, num_boxes3d, col_blocks, distance_threshold, mask_d, stream)); + + // memcpy device to host + thrust::host_vector mask_h(mask_d.size()); + thrust::copy(mask_d.begin(), mask_d.end(), mask_h.begin()); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream)); + + // generate keep_mask + std::vector remv_h(col_blocks); + thrust::host_vector keep_mask_h(keep_mask.size()); + std::size_t num_to_keep = 0; + for (std::size_t i = 0; i < num_boxes3d; i++) { + auto nblock = i / THREADS_PER_BLOCK_NMS; + auto inblock = i % THREADS_PER_BLOCK_NMS; + + if (!(remv_h[nblock] & (1ULL << inblock))) { + keep_mask_h[i] = true; + num_to_keep++; + std::uint64_t * p = &mask_h[0] + i * col_blocks; + for (std::size_t j = nblock; j < col_blocks; j++) { + remv_h[j] |= p[j]; + } + } else { + keep_mask_h[i] = false; + } + } + + // memcpy host to device + keep_mask = keep_mask_h; + + return num_to_keep; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp new file mode 100644 index 0000000000000..c1179a3f6673d --- /dev/null +++ b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -0,0 +1,104 @@ +// Copyright 2024 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 "lidar_transfusion/postprocess/non_maximum_suppression.hpp" + +#include +#include +#include + +namespace lidar_transfusion +{ + +void NonMaximumSuppression::setParameters(const NMSParams & params) +{ + assert(params.search_distance_2d_ >= 0.0); + assert(params.iou_threshold_ >= 0.0 && params.iou_threshold_ <= 1.0); + + params_ = params; + target_class_mask_ = classNamesToBooleanMask(params.target_class_names_); +} + +bool NonMaximumSuppression::isTargetLabel(const uint8_t label) +{ + if (label >= target_class_mask_.size()) { + return false; + } + return target_class_mask_.at(label); +} + +bool NonMaximumSuppression::isTargetPairObject( + const DetectedObject & object1, const DetectedObject & object2) +{ + const auto label1 = object_recognition_utils::getHighestProbLabel(object1.classification); + const auto label2 = object_recognition_utils::getHighestProbLabel(object2.classification); + + if (isTargetLabel(label1) && isTargetLabel(label2)) { + return true; + } + + const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; + const auto sqr_dist_2d = tier4_autoware_utils::calcSquaredDistance2d( + object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); + return sqr_dist_2d <= search_sqr_dist_2d; +} + +Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( + const std::vector & input_objects) +{ + // NOTE: row = target objects to be suppressed, col = source objects to be compared + Eigen::MatrixXd triangular_matrix = + Eigen::MatrixXd::Zero(input_objects.size(), input_objects.size()); + for (std::size_t target_i = 0; target_i < input_objects.size(); ++target_i) { + for (std::size_t source_i = 0; source_i < target_i; ++source_i) { + const auto & target_obj = input_objects.at(target_i); + const auto & source_obj = input_objects.at(source_i); + if (!isTargetPairObject(target_obj, source_obj)) { + continue; + } + + if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { + const double iou = object_recognition_utils::get2dIoU(target_obj, source_obj); + triangular_matrix(target_i, source_i) = iou; + // NOTE: If the target object has any objects with iou > iou_threshold, it + // will be suppressed regardless of later results. + if (iou > params_.iou_threshold_) { + break; + } + } + } + } + + return triangular_matrix; +} + +std::vector NonMaximumSuppression::apply( + const std::vector & input_objects) +{ + Eigen::MatrixXd iou_matrix = generateIoUMatrix(input_objects); + + std::vector output_objects; + output_objects.reserve(input_objects.size()); + for (std::size_t i = 0; i < input_objects.size(); ++i) { + const auto value = iou_matrix.row(i).maxCoeff(); + if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { + if (value <= params_.iou_threshold_) { + output_objects.emplace_back(input_objects.at(i)); + } + } + } + + return output_objects; +} +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu b/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu new file mode 100644 index 0000000000000..bca23e9961b40 --- /dev/null +++ b/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu @@ -0,0 +1,145 @@ +// Copyright 2024 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 "lidar_transfusion/postprocess/circle_nms_kernel.hpp" +#include "lidar_transfusion/postprocess/postprocess_kernel.hpp" + +#include +#include +#include + +namespace lidar_transfusion +{ +const size_t THREADS_PER_BLOCK = 256; + +struct is_score_greater +{ + is_score_greater(float t) : t_(t) {} + + __device__ bool operator()(const Box3D & b) { return b.score > t_; } + +private: + float t_{0.0}; +}; + +struct is_kept +{ + __device__ bool operator()(const bool keep) { return keep; } +}; + +struct score_greater +{ + __device__ bool operator()(const Box3D & lb, const Box3D & rb) { return lb.score > rb.score; } +}; + +__device__ inline float sigmoid(float x) +{ + return 1.0f / (1.0f + expf(-x)); +} + +__global__ void generateBoxes3D_kernel( + const float * __restrict__ cls_output, const float * __restrict__ box_output, + const float * __restrict__ dir_cls_output, const float voxel_size_x, const float voxel_size_y, + const float min_x_range, const float min_y_range, const int num_proposals, const int num_classes, + const int num_point_values, const float * __restrict__ yaw_norm_thresholds, + Box3D * __restrict__ det_boxes3d) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= num_proposals) { + return; + } + + int class_id = 0; + float max_score = cls_output[point_idx]; + +#pragma unroll + for (int i = 1; i < num_classes; i++) { + float score = cls_output[i * num_proposals + point_idx]; + if (score > max_score) { + max_score = score; + class_id = i; + } + } + + // yaw validation + const float yaw_sin = dir_cls_output[point_idx]; + const float yaw_cos = dir_cls_output[point_idx + num_proposals]; + const float yaw_norm = sqrtf(yaw_sin * yaw_sin + yaw_cos * yaw_cos); + + det_boxes3d[point_idx].label = class_id; + det_boxes3d[point_idx].score = yaw_norm >= yaw_norm_thresholds[class_id] ? max_score : 0.f; + det_boxes3d[point_idx].x = box_output[point_idx] * num_point_values * voxel_size_x + min_x_range; + det_boxes3d[point_idx].y = + box_output[point_idx + num_proposals] * num_point_values * voxel_size_y + min_y_range; + det_boxes3d[point_idx].z = box_output[point_idx + 2 * num_proposals]; + det_boxes3d[point_idx].width = expf(box_output[point_idx + 3 * num_proposals]); + det_boxes3d[point_idx].length = expf(box_output[point_idx + 4 * num_proposals]); + det_boxes3d[point_idx].height = expf(box_output[point_idx + 5 * num_proposals]); + det_boxes3d[point_idx].yaw = + atan2f(dir_cls_output[point_idx], dir_cls_output[point_idx + num_proposals]); +} + +PostprocessCuda::PostprocessCuda(const TransfusionConfig & config, cudaStream_t & stream) +: config_(config), stream_(stream) +{ + boxes3d_d_ = thrust::device_vector(config_.num_proposals_); + yaw_norm_thresholds_d_ = thrust::device_vector( + config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end()); +} + +// cspell: ignore divup +cudaError_t PostprocessCuda::generateDetectedBoxes3D_launch( + const float * cls_output, const float * box_output, const float * dir_cls_output, + std::vector & det_boxes3d, cudaStream_t stream) +{ + dim3 threads = {THREADS_PER_BLOCK}; + dim3 blocks = {divup(config_.num_proposals_, threads.x)}; + + generateBoxes3D_kernel<<>>( + cls_output, box_output, dir_cls_output, config_.voxel_x_size_, config_.voxel_y_size_, + config_.min_x_range_, config_.min_y_range_, config_.num_proposals_, config_.num_classes_, + config_.num_point_values_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), + thrust::raw_pointer_cast(boxes3d_d_.data())); + + // suppress by score + const auto num_det_boxes3d = thrust::count_if( + thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), + is_score_greater(config_.score_threshold_)); + if (num_det_boxes3d == 0) { + return cudaGetLastError(); + } + thrust::device_vector det_boxes3d_d(num_det_boxes3d); + thrust::copy_if( + thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), det_boxes3d_d.begin(), + is_score_greater(config_.score_threshold_)); + + // sort by score + thrust::sort(det_boxes3d_d.begin(), det_boxes3d_d.end(), score_greater()); + + // supress by NMS + thrust::device_vector final_keep_mask_d(num_det_boxes3d); + const auto num_final_det_boxes3d = + circleNMS(det_boxes3d_d, config_.circle_nms_dist_threshold_, final_keep_mask_d, stream); + thrust::device_vector final_det_boxes3d_d(num_final_det_boxes3d); + thrust::copy_if( + thrust::device, det_boxes3d_d.begin(), det_boxes3d_d.end(), final_keep_mask_d.begin(), + final_det_boxes3d_d.begin(), is_kept()); + + // memcpy device to host + det_boxes3d.resize(num_final_det_boxes3d); + thrust::copy(final_det_boxes3d_d.begin(), final_det_boxes3d_d.end(), det_boxes3d.begin()); + return cudaGetLastError(); +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp b/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp new file mode 100644 index 0000000000000..774b3a05d71c1 --- /dev/null +++ b/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp @@ -0,0 +1,117 @@ +// Copyright 2024 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 "lidar_transfusion/preprocess/pointcloud_densification.hpp" + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include + +namespace +{ + +boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, + const std::string & source_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_transfusion"), ex.what()); + return boost::none; + } +} + +Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t) +{ + Eigen::Affine3f a; + a.matrix() = tf2::transformToEigen(t).matrix().cast(); + return a; +} + +} // namespace + +namespace lidar_transfusion +{ + +PointCloudDensification::PointCloudDensification( + const DensificationParam & param, cudaStream_t & stream) +: param_(param), stream_(stream) +{ +} + +bool PointCloudDensification::enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer) +{ + const auto header = pointcloud_msg.header; + + if (param_.pointcloud_cache_size() > 1) { + auto transform_world2current = + getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp); + if (!transform_world2current) { + return false; + } + auto affine_world2current = transformToEigen(transform_world2current.get()); + + enqueue(pointcloud_msg, affine_world2current); + } else { + enqueue(pointcloud_msg, Eigen::Affine3f::Identity()); + } + + dequeue(); + + return true; +} + +void PointCloudDensification::enqueue( + const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current) +{ + affine_world2current_ = affine_world2current; + current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds(); + + assert(sizeof(uint8_t) * msg.width * msg.height * msg.point_step % sizeof(float) == 1); + auto points_d = cuda::make_unique( + sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(float)); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + points_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step, + cudaMemcpyHostToDevice, stream_)); + + PointCloudWithTransform pointcloud = { + std::move(points_d), msg.header, msg.width * msg.height, affine_world2current.inverse()}; + + pointcloud_cache_.push_front(std::move(pointcloud)); +} + +void PointCloudDensification::dequeue() +{ + if (pointcloud_cache_.size() > param_.pointcloud_cache_size()) { + pointcloud_cache_.pop_back(); + } +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu new file mode 100644 index 0000000000000..b8f9ae998fd4f --- /dev/null +++ b/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -0,0 +1,208 @@ +// Copyright 2024 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. +/* + * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * 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 "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" + +namespace lidar_transfusion +{ + +PreprocessCuda::PreprocessCuda(const TransfusionConfig & config, cudaStream_t & stream) +: stream_(stream), config_(config) +{ + mask_size_ = config_.grid_z_size_ * config_.grid_y_size_ * config_.grid_x_size_; + voxels_size_ = config_.grid_z_size_ * config_.grid_y_size_ * config_.grid_x_size_ * + config_.max_num_points_per_pillar_ * config_.num_point_feature_size_ + + 1; + mask_ = cuda::make_unique(mask_size_); + voxels_ = cuda::make_unique(voxels_size_); +} + +void PreprocessCuda::generateVoxels( + float * points, unsigned int points_size, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs) +{ + cuda::clear_async(mask_.get(), mask_size_, stream_); + cuda::clear_async(voxels_.get(), voxels_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + CHECK_CUDA_ERROR(generateVoxels_random_launch(points, points_size, mask_.get(), voxels_.get())); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + CHECK_CUDA_ERROR(generateBaseFeatures_launch( + mask_.get(), voxels_.get(), pillar_num, voxel_features, voxel_num, voxel_idxs)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); +} + +__global__ void generateVoxels_random_kernel( + float * points, unsigned int points_size, float min_x_range, float max_x_range, float min_y_range, + float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, + float pillar_z_size, int grid_y_size, int grid_x_size, int points_per_voxel, unsigned int * mask, + float * voxels) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= points_size) return; + + float x = points[point_idx * 5]; + float y = points[point_idx * 5 + 1]; + float z = points[point_idx * 5 + 2]; + float i = points[point_idx * 5 + 3]; + float t = points[point_idx * 5 + 4]; + + if ( + x <= min_x_range || x >= max_x_range || y <= min_y_range || y >= max_y_range || + z <= min_z_range || z >= max_z_range) + return; + + int voxel_idx = floorf((x - min_x_range) / pillar_x_size); + int voxel_idy = floorf((y - min_y_range) / pillar_y_size); + unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + + unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); + + if (point_id >= points_per_voxel) return; + float * address = voxels + (voxel_index * points_per_voxel + point_id) * 5; + atomicExch(address + 0, x); + atomicExch(address + 1, y); + atomicExch(address + 2, z); + atomicExch(address + 3, i); + atomicExch(address + 4, t); +} + +cudaError_t PreprocessCuda::generateVoxels_random_launch( + float * points, unsigned int points_size, unsigned int * mask, float * voxels) +{ + int threadNum = config_.threads_for_voxel_; + dim3 blocks((points_size + threadNum - 1) / threadNum); + dim3 threads(threadNum); + generateVoxels_random_kernel<<>>( + points, points_size, config_.min_x_range_, config_.max_x_range_, config_.min_y_range_, + config_.max_y_range_, config_.min_z_range_, config_.max_z_range_, config_.voxel_x_size_, + config_.voxel_y_size_, config_.voxel_z_size_, config_.grid_y_size_, config_.grid_x_size_, + config_.points_per_voxel_, mask, voxels); + cudaError_t err = cudaGetLastError(); + return err; +} + +__global__ void generateBaseFeatures_kernel( + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, float points_per_voxel, + float max_voxels, unsigned int * pillar_num, float * voxel_features, unsigned int * voxel_num, + unsigned int * voxel_idxs) +{ + unsigned int voxel_idx = blockIdx.x * blockDim.x + threadIdx.x; + unsigned int voxel_idy = blockIdx.y * blockDim.y + threadIdx.y; + + if (voxel_idx >= grid_x_size || voxel_idy >= grid_y_size) return; + + unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + unsigned int count = mask[voxel_index]; + if (!(count > 0)) return; + count = count < points_per_voxel ? count : points_per_voxel; + + unsigned int current_pillarId = 0; + current_pillarId = atomicAdd(pillar_num, 1); + if (current_pillarId >= max_voxels) return; + + voxel_num[current_pillarId] = count; + + uint4 idx = {0, 0, voxel_idy, voxel_idx}; + ((uint4 *)voxel_idxs)[current_pillarId] = idx; + + for (int i = 0; i < count; i++) { + int inIndex = voxel_index * points_per_voxel + i; + int outIndex = current_pillarId * points_per_voxel + i; + voxel_features[outIndex * 5] = voxels[inIndex * 5]; + voxel_features[outIndex * 5 + 1] = voxels[inIndex * 5 + 1]; + voxel_features[outIndex * 5 + 2] = voxels[inIndex * 5 + 2]; + voxel_features[outIndex * 5 + 3] = voxels[inIndex * 5 + 3]; + voxel_features[outIndex * 5 + 4] = voxels[inIndex * 5 + 4]; + } + + // clear buffer for next infer + atomicExch(mask + voxel_index, 0); +} + +// create 4 channels +cudaError_t PreprocessCuda::generateBaseFeatures_launch( + unsigned int * mask, float * voxels, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs) +{ + dim3 threads = {32, 32}; + dim3 blocks = {divup(config_.grid_x_size_, threads.x), divup(config_.grid_y_size_, threads.y)}; + + generateBaseFeatures_kernel<<>>( + mask, voxels, config_.grid_y_size_, config_.grid_x_size_, config_.points_per_voxel_, + config_.max_voxels_, pillar_num, voxel_features, voxel_num, voxel_idxs); + cudaError_t err = cudaGetLastError(); + return err; +} + +__global__ void generateSweepPoints_kernel( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform_array, int num_features, float * output_points) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= points_size) return; + + const float input_x = input_points[point_idx * input_point_step + 0]; + const float input_y = input_points[point_idx * input_point_step + 1]; + const float input_z = input_points[point_idx * input_point_step + 2]; + const float intensity = input_points[point_idx * input_point_step + 3]; + + output_points[point_idx * num_features] = transform_array[0] * input_x + + transform_array[4] * input_y + + transform_array[8] * input_z + transform_array[12]; + output_points[point_idx * num_features + 1] = transform_array[1] * input_x + + transform_array[5] * input_y + + transform_array[9] * input_z + transform_array[13]; + output_points[point_idx * num_features + 2] = transform_array[2] * input_x + + transform_array[6] * input_y + + transform_array[10] * input_z + transform_array[14]; + output_points[point_idx * num_features + 3] = intensity; + output_points[point_idx * num_features + 4] = time_lag; +} + +cudaError_t PreprocessCuda::generateSweepPoints_launch( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform_array, float * output_points) +{ + int threadNum = config_.threads_for_voxel_; + dim3 blocks((points_size + threadNum - 1) / threadNum); + dim3 threads(threadNum); + + generateSweepPoints_kernel<<>>( + input_points, points_size, input_point_step, time_lag, transform_array, + config_.num_point_feature_size_, output_points); + + cudaError_t err = cudaGetLastError(); + return err; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp b/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp new file mode 100644 index 0000000000000..7072e8491af66 --- /dev/null +++ b/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp @@ -0,0 +1,142 @@ +// Copyright 2024 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 "lidar_transfusion/preprocess/voxel_generator.hpp" + +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" + +#include + +#include + +namespace lidar_transfusion +{ + +std::ostream & operator<<(std::ostream & os, const CloudInfo & info) +{ + os << "x_offset: " << static_cast(info.x_offset) << std::endl; + os << "y_offset: " << static_cast(info.y_offset) << std::endl; + os << "z_offset: " << static_cast(info.z_offset) << std::endl; + os << "intensity_offset: " << static_cast(info.intensity_offset) << std::endl; + os << "x_datatype: " << static_cast(info.x_datatype) << std::endl; + os << "y_datatype: " << static_cast(info.y_datatype) << std::endl; + os << "z_datatype: " << static_cast(info.z_datatype) << std::endl; + os << "intensity_datatype: " << static_cast(info.intensity_datatype) << std::endl; + os << "x_count: " << static_cast(info.x_count) << std::endl; + os << "y_count: " << static_cast(info.y_count) << std::endl; + os << "z_count: " << static_cast(info.z_count) << std::endl; + os << "intensity_count: " << static_cast(info.intensity_count) << std::endl; + os << "point_step: " << static_cast(info.point_step) << std::endl; + os << "is_bigendian: " << static_cast(info.is_bigendian) << std::endl; + return os; +} + +VoxelGenerator::VoxelGenerator( + const DensificationParam & densification_param, const TransfusionConfig & config, + cudaStream_t & stream) +: config_(config), stream_(stream) +{ + pd_ptr_ = std::make_unique(densification_param, stream_); + pre_ptr_ = std::make_unique(config_, stream_); + cloud_data_d_ = cuda::make_unique(config_.cloud_capacity_ * MAX_CLOUD_STEP_SIZE); + affine_past2current_d_ = cuda::make_unique(AFF_MAT_SIZE); +} + +bool VoxelGenerator::enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer) +{ + return pd_ptr_->enqueuePointCloud(msg, tf_buffer); +} + +std::size_t VoxelGenerator::generateSweepPoints( + const sensor_msgs::msg::PointCloud2 & msg, cuda::unique_ptr & points_d) +{ + if (!is_initialized_) { + initCloudInfo(msg); + std::stringstream ss; + ss << "Input point cloud information: " << std::endl << cloud_info_; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_transfusion"), ss.str()); + + CloudInfo default_cloud_info; + if (cloud_info_ != default_cloud_info) { + ss << "Expected point cloud information: " << std::endl << default_cloud_info; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("lidar_transfusion"), ss.str()); + throw std::runtime_error("Input point cloud has unsupported format"); + } + is_initialized_ = true; + } + + size_t point_counter{0}; + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + for (auto pc_cache_iter = pd_ptr_->getPointCloudCacheIter(); !pd_ptr_->isCacheEnd(pc_cache_iter); + pc_cache_iter++) { + auto sweep_num_points = pc_cache_iter->num_points; + if (point_counter + sweep_num_points >= config_.cloud_capacity_) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Exceeding cloud capacity. Used " + << pd_ptr_->getIdx(pc_cache_iter) << " out of " + << pd_ptr_->getCacheSize() << " sweep(s)"); + break; + } + auto shift = point_counter * config_.num_point_feature_size_; + + auto affine_past2current = + pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world; + static_assert(std::is_same::value); + static_assert(!Eigen::Matrix4f::IsRowMajor, "matrices should be col-major."); + + float time_lag = static_cast( + pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_cache_iter->header.stamp).seconds()); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + affine_past2current_d_.get(), affine_past2current.data(), AFF_MAT_SIZE * sizeof(float), + cudaMemcpyHostToDevice, stream_)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + pre_ptr_->generateSweepPoints_launch( + pc_cache_iter->points_d.get(), sweep_num_points, cloud_info_.point_step / sizeof(float), + time_lag, affine_past2current_d_.get(), points_d.get() + shift); + point_counter += sweep_num_points; + } + + return point_counter; +} + +void VoxelGenerator::initCloudInfo(const sensor_msgs::msg::PointCloud2 & msg) +{ + std::tie(cloud_info_.x_offset, cloud_info_.x_datatype, cloud_info_.x_count) = + getFieldInfo(msg, "x"); + std::tie(cloud_info_.y_offset, cloud_info_.y_datatype, cloud_info_.y_count) = + getFieldInfo(msg, "y"); + std::tie(cloud_info_.z_offset, cloud_info_.z_datatype, cloud_info_.z_count) = + getFieldInfo(msg, "z"); + std::tie( + cloud_info_.intensity_offset, cloud_info_.intensity_datatype, cloud_info_.intensity_count) = + getFieldInfo(msg, "intensity"); + cloud_info_.point_step = msg.point_step; + cloud_info_.is_bigendian = msg.is_bigendian; +} + +std::tuple VoxelGenerator::getFieldInfo( + const sensor_msgs::msg::PointCloud2 & msg, const std::string & field_name) +{ + for (const auto & field : msg.fields) { + if (field.name == field_name) { + return std::make_tuple(field.offset, field.datatype, field.count); + } + } + throw std::runtime_error("Missing field: " + field_name); +} +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/ros_utils.cpp b/perception/lidar_transfusion/lib/ros_utils.cpp new file mode 100644 index 0000000000000..0ecb34faf732d --- /dev/null +++ b/perception/lidar_transfusion/lib/ros_utils.cpp @@ -0,0 +1,83 @@ +// Copyright 2024 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 "lidar_transfusion/ros_utils.hpp" + +#include +#include +#include + +namespace lidar_transfusion +{ + +using Label = autoware_perception_msgs::msg::ObjectClassification; + +void box3DToDetectedObject( + const Box3D & box3d, const std::vector & class_names, + autoware_perception_msgs::msg::DetectedObject & obj) +{ + obj.existence_probability = box3d.score; + + // classification + autoware_perception_msgs::msg::ObjectClassification classification; + classification.probability = 1.0f; + if (box3d.label >= 0 && static_cast(box3d.label) < class_names.size()) { + classification.label = getSemanticType(class_names[box3d.label]); + } else { + classification.label = Label::UNKNOWN; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Unexpected label: UNKNOWN is set."); + } + + if (object_recognition_utils::isCarLikeVehicle(classification.label)) { + obj.kinematics.orientation_availability = + autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + } + + obj.classification.emplace_back(classification); + + // pose and shape + // mmdet3d yaw format to ros yaw format + float yaw = box3d.yaw + tier4_autoware_utils::pi / 2; + obj.kinematics.pose_with_covariance.pose.position = + tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); + obj.kinematics.pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw); + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions = + tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); +} + +uint8_t getSemanticType(const std::string & class_name) +{ + if (class_name == "CAR") { + return Label::CAR; + } else if (class_name == "TRUCK") { + return Label::TRUCK; + } else if (class_name == "BUS") { + return Label::BUS; + } else if (class_name == "TRAILER") { + return Label::TRAILER; + } else if (class_name == "MOTORCYCLE") { + return Label::MOTORCYCLE; + } else if (class_name == "BICYCLE") { + return Label::BICYCLE; + } else if (class_name == "PEDESTRIAN") { + return Label::PEDESTRIAN; + } else { // CONSTRUCTION_VEHICLE, BARRIER, TRAFFIC_CONE + return Label::UNKNOWN; + } +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/transfusion_trt.cpp b/perception/lidar_transfusion/lib/transfusion_trt.cpp new file mode 100644 index 0000000000000..c876c0906162b --- /dev/null +++ b/perception/lidar_transfusion/lib/transfusion_trt.cpp @@ -0,0 +1,205 @@ +// Copyright 2024 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 "lidar_transfusion/transfusion_trt.hpp" + +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/transfusion_config.hpp" + +#include + +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +TransfusionTRT::TransfusionTRT( + const NetworkParam & network_param, const DensificationParam & densification_param, + const TransfusionConfig & config) +: config_(config) +{ + network_trt_ptr_ = std::make_unique(config_); + + network_trt_ptr_->init( + network_param.onnx_path(), network_param.engine_path(), network_param.trt_precision()); + vg_ptr_ = std::make_unique(densification_param, config_, stream_); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("processing/inner"); + initPtr(); + + CHECK_CUDA_ERROR(cudaStreamCreate(&stream_)); +} + +TransfusionTRT::~TransfusionTRT() +{ + if (stream_) { + cudaStreamSynchronize(stream_); + cudaStreamDestroy(stream_); + } +} + +void TransfusionTRT::initPtr() +{ + // point cloud to voxels + voxel_features_size_ = + config_.max_voxels_ * config_.max_num_points_per_pillar_ * config_.num_point_feature_size_; + voxel_num_size_ = config_.max_voxels_; + voxel_idxs_size_ = config_.max_voxels_ * config_.num_point_values_; + + // output of TRT -- input of post-process + cls_size_ = config_.num_proposals_ * config_.num_classes_; + box_size_ = config_.num_proposals_ * config_.num_box_values_; + dir_cls_size_ = config_.num_proposals_ * 2; // x, y + cls_output_d_ = cuda::make_unique(cls_size_); + box_output_d_ = cuda::make_unique(box_size_); + dir_cls_output_d_ = cuda::make_unique(dir_cls_size_); + + params_input_d_ = cuda::make_unique(); + voxel_features_d_ = cuda::make_unique(voxel_features_size_); + voxel_num_d_ = cuda::make_unique(voxel_num_size_); + voxel_idxs_d_ = cuda::make_unique(voxel_idxs_size_); + points_d_ = cuda::make_unique(config_.cloud_capacity_ * config_.num_point_feature_size_); + pre_ptr_ = std::make_unique(config_, stream_); + post_ptr_ = std::make_unique(config_, stream_); +} + +bool TransfusionTRT::detect( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer, + std::vector & det_boxes3d, std::unordered_map & proc_timing) +{ + stop_watch_ptr_->toc("processing/inner", true); + if (!preprocess(msg, tf_buffer)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to preprocess and skip to detect."); + return false; + } + proc_timing.emplace( + "debug/processing_time/preprocess_ms", stop_watch_ptr_->toc("processing/inner", true)); + + if (!inference()) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to inference and skip to detect."); + return false; + } + proc_timing.emplace( + "debug/processing_time/inference_ms", stop_watch_ptr_->toc("processing/inner", true)); + + if (!postprocess(det_boxes3d)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to postprocess and skip to detect."); + return false; + } + proc_timing.emplace( + "debug/processing_time/postprocess_ms", stop_watch_ptr_->toc("processing/inner", true)); + + return true; +} + +bool TransfusionTRT::preprocess( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer) +{ + if (!vg_ptr_->enqueuePointCloud(msg, tf_buffer)) { + return false; + } + + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_.cloud_capacity_ * config_.num_point_feature_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + const auto count = vg_ptr_->generateSweepPoints(msg, points_d_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_transfusion"), "Generated sweep points: " << count); + + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + unsigned int params_input; + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + ¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost, stream_)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + if (params_input < config_.min_voxel_size_) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), + "Too few voxels (" << params_input << ") for actual optimization profile (" + << config_.min_voxel_size_ << ")"); + return false; + } + + if (params_input > config_.max_voxels_) { + params_input = config_.max_voxels_; + } + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Generated input voxels: " << params_input); + + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::voxels), voxel_features_d_.get()); + network_trt_ptr_->context->setInputShape( + network_trt_ptr_->getTensorName(NetworkIO::voxels), + nvinfer1::Dims3{ + static_cast(params_input), static_cast(config_.max_num_points_per_pillar_), + static_cast(config_.num_point_feature_size_)}); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::num_points), voxel_num_d_.get()); + network_trt_ptr_->context->setInputShape( + network_trt_ptr_->getTensorName(NetworkIO::num_points), + nvinfer1::Dims{1, {static_cast(params_input)}}); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::coors), voxel_idxs_d_.get()); + network_trt_ptr_->context->setInputShape( + network_trt_ptr_->getTensorName(NetworkIO::coors), + nvinfer1::Dims2{ + static_cast(params_input), static_cast(config_.num_point_values_)}); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::cls_score), cls_output_d_.get()); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::bbox_pred), box_output_d_.get()); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::dir_pred), dir_cls_output_d_.get()); + return true; +} + +bool TransfusionTRT::inference() +{ + auto status = network_trt_ptr_->context->enqueueV3(stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + if (!status) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to enqueue and skip to detect."); + return false; + } + return true; +} + +bool TransfusionTRT::postprocess(std::vector & det_boxes3d) +{ + CHECK_CUDA_ERROR(post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + return true; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/package.xml b/perception/lidar_transfusion/package.xml new file mode 100644 index 0000000000000..29643d0957eb1 --- /dev/null +++ b/perception/lidar_transfusion/package.xml @@ -0,0 +1,34 @@ + + + + lidar_transfusion + 1.0.0 + The lidar_transfusion package + Amadeusz Szymko + Kenzo Lobos-Tsunekawa + Satoshi Tanaka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_perception_msgs + launch_ros + object_recognition_utils + pcl_ros + rclcpp + rclcpp_components + tensorrt_common + tf2_eigen + tf2_geometry_msgs + tf2_sensor_msgs + tier4_autoware_utils + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/lidar_transfusion/schema/detection_class_remapper.schema.json b/perception/lidar_transfusion/schema/detection_class_remapper.schema.json new file mode 100644 index 0000000000000..8aaefa295fcc0 --- /dev/null +++ b/perception/lidar_transfusion/schema/detection_class_remapper.schema.json @@ -0,0 +1,72 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Detection Class Remapper", + "type": "object", + "definitions": { + "detection_class_remapper": { + "type": "object", + "properties": { + "allow_remapping_by_area_matrix": { + "type": "array", + "items": { + "type": "integer" + }, + "description": "Whether to allow remapping of classes. The order of 8x8 matrix classes comes from ObjectClassification msg.", + "default": [ + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, + 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 + ], + "minItems": 64, + "maxItems": 64 + }, + "min_area_matrix": { + "type": "array", + "items": { + "type": "number" + }, + "description": "Minimum area for specific class to consider class remapping.", + "default": [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 12.1, 0.0, 36.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 36.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 36.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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ], + "minItems": 64, + "maxItems": 64 + }, + "max_area_matrix": { + "type": "array", + "items": { + "type": "number" + }, + "description": "Maximum area for specific class to consider class remapping.", + "default": [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 36.0, 0.0, 999.999, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 999.999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 999.999, 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.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ], + "minItems": 64, + "maxItems": 64 + } + }, + "required": ["allow_remapping_by_area_matrix", "min_area_matrix", "max_area_matrix"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/detection_class_remapper" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/lidar_transfusion/schema/transfusion.schema.json b/perception/lidar_transfusion/schema/transfusion.schema.json new file mode 100644 index 0000000000000..41d8d887236a8 --- /dev/null +++ b/perception/lidar_transfusion/schema/transfusion.schema.json @@ -0,0 +1,160 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for LiDAR TransFusion Node", + "type": "object", + "definitions": { + "transfusion": { + "type": "object", + "properties": { + "class_names": { + "type": "array", + "items": { + "type": "string" + }, + "description": "An array of class names will be predicted.", + "default": ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"], + "uniqueItems": true + }, + "trt_precision": { + "type": "string", + "description": "A precision of TensorRT engine.", + "default": "fp16", + "enum": ["fp16", "fp32"] + }, + "voxels_num": { + "type": "array", + "items": { + "type": "integer" + }, + "description": "A maximum number of voxels [min, opt, max].", + "default": [5000, 30000, 60000] + }, + "point_cloud_range": { + "type": "array", + "items": { + "type": "number" + }, + "description": "An array of distance ranges of each class.", + "default": [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0], + "minItems": 6, + "maxItems": 6 + }, + "voxel_size": { + "type": "array", + "items": { + "type": "number" + }, + "description": "Voxels size [x, y, z].", + "default": [0.3, 0.3, 8.0], + "minItems": 3, + "maxItems": 3 + }, + "onnx_path": { + "type": "string", + "description": "A path to ONNX model file.", + "default": "$(var model_path)/transfusion.onnx", + "pattern": "\\.onnx$" + }, + "engine_path": { + "type": "string", + "description": "A path to TensorRT engine file.", + "default": "$(var model_path)/transfusion.engine", + "pattern": "\\.engine$" + }, + "down_sample_factor": { + "type": "integer", + "description": "A scale factor of downsampling points", + "default": 1, + "minimum": 1 + }, + "densification_num_past_frames": { + "type": "integer", + "description": "A number of past frames to be considered as same input frame.", + "default": 1, + "minimum": 0 + }, + "densification_world_frame_id": { + "type": "string", + "description": "A name of frame id where world coordinates system is defined with respect to.", + "default": "map" + }, + "circle_nms_dist_threshold": { + "type": "number", + "description": "A distance threshold between detections in NMS.", + "default": 0.5, + "minimum": 0.0 + }, + "iou_nms_target_class_names": { + "type": "array", + "items": { + "type": "string" + }, + "description": "An array of class names to be target in NMS.", + "default": ["CAR"], + "uniqueItems": true + }, + "iou_nms_search_distance_2d": { + "type": "number", + "description": "A maximum distance value to search the nearest objects.", + "default": 10.0, + "minimum": 0.0 + }, + "iou_nms_threshold": { + "type": "number", + "description": "A threshold value of NMS using IoU score.", + "default": 0.1, + "minimum": 0.0, + "maximum": 1.0 + }, + "yaw_norm_thresholds": { + "type": "array", + "items": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0 + }, + "description": "A thresholds array of direction vectors norm, all of objects with vector norm less than this threshold are ignored.", + "default": [0.3, 0.3, 0.3, 0.3, 0.0] + }, + "score_threshold": { + "type": "number", + "description": "A threshold value of confidence score, all of objects with score less than this threshold are ignored.", + "default": 0.2, + "minimum": 0.0 + } + }, + "required": [ + "class_names", + "trt_precision", + "voxels_num", + "point_cloud_range", + "voxel_size", + "onnx_path", + "engine_path", + "densification_num_past_frames", + "densification_world_frame_id", + "circle_nms_dist_threshold", + "iou_nms_target_class_names", + "iou_nms_search_distance_2d", + "iou_nms_threshold", + "yaw_norm_thresholds", + "score_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/transfusion" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp new file mode 100644 index 0000000000000..9313015002973 --- /dev/null +++ b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp @@ -0,0 +1,178 @@ +// Copyright 2024 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 "lidar_transfusion/lidar_transfusion_node.hpp" + +#include "lidar_transfusion/utils.hpp" + +namespace lidar_transfusion +{ + +LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) +: Node("lidar_transfusion", options), tf_buffer_(this->get_clock()) +{ + auto descriptor = rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); + // network + class_names_ = this->declare_parameter>("class_names", descriptor); + const std::string trt_precision = + this->declare_parameter("trt_precision", descriptor); + const auto voxels_num = this->declare_parameter>("voxels_num", descriptor); + const auto point_cloud_range = + this->declare_parameter>("point_cloud_range", descriptor); + const auto voxel_size = this->declare_parameter>("voxel_size", descriptor); + const std::string onnx_path = this->declare_parameter("onnx_path", descriptor); + const std::string engine_path = this->declare_parameter("engine_path", descriptor); + + if (point_cloud_range.size() != 6) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), + "The size of point_cloud_range != 6: use the default parameters."); + } + if (voxel_size.size() != 3) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), + "The size of voxel_size != 3: use the default parameters."); + } + + // pre-process + const std::string densification_world_frame_id = + this->declare_parameter("densification_world_frame_id", descriptor); + const int densification_num_past_frames = + this->declare_parameter("densification_num_past_frames", descriptor); + + // post-process + const float circle_nms_dist_threshold = + static_cast(this->declare_parameter("circle_nms_dist_threshold", descriptor)); + { // IoU NMS + NMSParams p; + p.nms_type_ = NMS_TYPE::IoU_BEV; + p.target_class_names_ = + this->declare_parameter>("iou_nms_target_class_names", descriptor); + p.search_distance_2d_ = + this->declare_parameter("iou_nms_search_distance_2d", descriptor); + p.iou_threshold_ = this->declare_parameter("iou_nms_threshold", descriptor); + iou_bev_nms_.setParameters(p); + } + const auto yaw_norm_thresholds = + this->declare_parameter>("yaw_norm_thresholds", descriptor); + const float score_threshold = + static_cast(this->declare_parameter("score_threshold", descriptor)); + + NetworkParam network_param(onnx_path, engine_path, trt_precision); + DensificationParam densification_param( + densification_world_frame_id, densification_num_past_frames); + TransfusionConfig config( + voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, + score_threshold); + + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix", descriptor); + const auto min_area_matrix = + this->declare_parameter>("min_area_matrix", descriptor); + const auto max_area_matrix = + this->declare_parameter>("max_area_matrix", descriptor); + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + detector_ptr_ = std::make_unique(network_param, densification_param, config); + + cloud_sub_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), + std::bind(&LidarTransfusionNode::cloudCallback, this, std::placeholders::_1)); + + objects_pub_ = this->create_publisher( + "~/output/objects", rclcpp::QoS(1)); + + published_time_pub_ = std::make_unique(this); + + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ptr_ = std::make_unique(this, this->get_name()); + stop_watch_ptr_->tic("cyclic"); + stop_watch_ptr_->tic("processing/total"); + } + + if (this->declare_parameter("build_only", false, descriptor)) { + RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); + rclcpp::shutdown(); + } +} + +void LidarTransfusionNode::cloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + const auto objects_sub_count = + objects_pub_->get_subscription_count() + objects_pub_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + if (stop_watch_ptr_) { + stop_watch_ptr_->toc("processing/total", true); + } + + std::vector det_boxes3d; + std::unordered_map proc_timing; + bool is_success = detector_ptr_->detect(*msg, tf_buffer_, det_boxes3d, proc_timing); + if (!is_success) { + return; + } + + std::vector raw_objects; + raw_objects.reserve(det_boxes3d.size()); + for (const auto & box3d : det_boxes3d) { + autoware_perception_msgs::msg::DetectedObject obj; + box3DToDetectedObject(box3d, class_names_, obj); + raw_objects.emplace_back(obj); + } + + autoware_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = msg->header; + output_msg.objects = iou_bev_nms_.apply(raw_objects); + + detection_class_remapper_.mapClasses(output_msg); + + if (objects_sub_count > 0) { + objects_pub_->publish(output_msg); + published_time_pub_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); + } + + // add processing time for debug + if (debug_publisher_ptr_ && stop_watch_ptr_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing/total", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_msg.header.stamp).nanoseconds())) + .count(); + debug_publisher_ptr_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_ptr_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + debug_publisher_ptr_->publish( + "debug/processing_time/total_ms", processing_time_ms); + for (const auto & [topic, time_ms] : proc_timing) { + debug_publisher_ptr_->publish(topic, time_ms); + } + } +} + +} // namespace lidar_transfusion + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(lidar_transfusion::LidarTransfusionNode) diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt deleted file mode 100644 index 786446457143f..0000000000000 --- a/perception/map_based_prediction/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(map_based_prediction) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) - -find_package(glog REQUIRED) - -include_directories( - SYSTEM - ${EIGEN3_INCLUDE_DIR} -) - -ament_auto_add_library(map_based_prediction_node SHARED - src/map_based_prediction_node.cpp - src/path_generator.cpp - src/debug.cpp -) - -target_link_libraries(map_based_prediction_node glog::glog) - -rclcpp_components_register_node(map_based_prediction_node - PLUGIN "map_based_prediction::MapBasedPredictionNode" - EXECUTABLE map_based_prediction -) - -## Tests -if(BUILD_TESTING) - find_package(ament_cmake_ros REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) - - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - - file(GLOB_RECURSE test_files test/**/*.cpp) - ament_add_ros_isolated_gtest(test_map_based_prediction ${test_files}) - - target_link_libraries(test_map_based_prediction - map_based_prediction_node - ) -endif() - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) diff --git a/perception/map_based_prediction/launch/map_based_prediction.launch.xml b/perception/map_based_prediction/launch/map_based_prediction.launch.xml deleted file mode 100644 index 7f81971b2f8d7..0000000000000 --- a/perception/map_based_prediction/launch/map_based_prediction.launch.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml deleted file mode 100644 index 70ad1ec8519d9..0000000000000 --- a/perception/map_based_prediction/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - map_based_prediction - 0.1.0 - This package implements a map_based_prediction - Tomoya Kimura - Yoshi Ri - Takayuki Murooka - Kyoichi Sugahara - Kotaro Uetake - Apache License 2.0 - - ament_cmake - autoware_cmake - - autoware_perception_msgs - glog - interpolation - lanelet2_extension - motion_utils - rclcpp - rclcpp_components - tf2 - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - unique_identifier_msgs - visualization_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/perception/map_based_prediction/src/debug.cpp b/perception/map_based_prediction/src/debug.cpp deleted file mode 100644 index f78879ba209a1..0000000000000 --- a/perception/map_based_prediction/src/debug.cpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2021 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 "map_based_prediction/map_based_prediction_node.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" - -namespace map_based_prediction -{ -visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( - const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num) -{ - visualization_msgs::msg::Marker marker{}; - marker.header.frame_id = "map"; - marker.ns = "maneuver"; - - marker.id = static_cast(obj_num); - marker.lifetime = rclcpp::Duration::from_seconds(1.0); - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = object.kinematics.pose_with_covariance.pose; - marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.0, 1.0); - - // Color by maneuver - double r = 0.0; - double g = 0.0; - double b = 0.0; - if (maneuver == Maneuver::LEFT_LANE_CHANGE) { - g = 1.0; - } else if (maneuver == Maneuver::RIGHT_LANE_CHANGE) { - r = 1.0; - } else { - b = 1.0; - } - marker.color = tier4_autoware_utils::createMarkerColor(r, g, b, 0.8); - - return marker; -} -} // namespace map_based_prediction diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index f166e83e136af..e6a0b6242a168 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -134,6 +134,9 @@ void TrackerObjectDebugger::process() { if (!is_initialized_) return; + // Check if object_data_list_ is empty + if (object_data_list_.empty()) return; + // update uuid_int for (auto & object_data : object_data_list_) { current_ids_.insert(object_data.uuid_int); diff --git a/planning/.pages b/planning/.pages index 544e4c0cb2772..ac13a5a66fcfd 100644 --- a/planning/.pages +++ b/planning/.pages @@ -1,41 +1,40 @@ nav: - 'Introduction': planning - 'Behavior Path Planner': - - 'About Behavior Path': planning/behavior_path_planner + - 'About Behavior Path': planning/autoware_behavior_path_planner - 'Concept': - - 'Planner Manager': planning/behavior_path_planner/docs/behavior_path_planner_manager_design - - 'Scene Module Interface': planning/behavior_path_planner/docs/behavior_path_planner_interface_design - - 'Path Generation': planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design - - 'Collision Assessment/Safety Check': planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check - - 'Dynamic Drivable Area': planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design - - 'Turn Signal': planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design + - 'Planner Manager': planning/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design + - 'Scene Module Interface': planning/autoware_behavior_path_planner/docs/behavior_path_planner_interface_design + - 'Path Generation': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design + - 'Collision Assessment/Safety Check': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check + - 'Dynamic Drivable Area': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design + - 'Turn Signal': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design - 'Scene Module': - - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module + - 'Avoidance by Lane Change': planning/autoware_behavior_path_avoidance_by_lane_change_module - 'Dynamic Obstacle Avoidance': planning/autoware_behavior_path_dynamic_obstacle_avoidance_module - - 'Goal Planner': planning/behavior_path_goal_planner_module - - 'Lane Change': planning/behavior_path_lane_change_module - - 'Side Shift': planning/behavior_path_side_shift_module - - 'Start Planner': planning/behavior_path_start_planner_module + - 'Goal Planner': planning/autoware_behavior_path_goal_planner_module + - 'Lane Change': planning/autoware_behavior_path_lane_change_module + - 'Side Shift': planning/autoware_behavior_path_side_shift_module + - 'Start Planner': planning/autoware_behavior_path_start_planner_module - 'Static Obstacle Avoidance': planning/autoware_behavior_path_static_obstacle_avoidance_module - 'Behavior Velocity Planner': - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner - 'Template for Custom Module': planning/autoware_behavior_velocity_template_module - 'Available Module': - - 'Blind Spot': planning/behavior_velocity_blind_spot_module - - 'Crosswalk': planning/behavior_velocity_crosswalk_module + - 'Blind Spot': planning/autoware_behavior_velocity_blind_spot_module + - 'Crosswalk': planning/autoware_behavior_velocity_crosswalk_module - 'Detection Area': planning/behavior_velocity_detection_area_module - 'Dynamic Obstacle Stop': planning/behavior_velocity_dynamic_obstacle_stop_module - - 'Intersection': planning/behavior_velocity_intersection_module + - 'Intersection': planning/autoware_behavior_velocity_intersection_module - 'No Drivable Lane': planning/behavior_velocity_no_drivable_lane_module - 'No Stopping Area': planning/behavior_velocity_no_stopping_area_module - - 'Occlusion Spot': planning/behavior_velocity_occlusion_spot_module - - 'Out of Lane': planning/behavior_velocity_out_of_lane_module - - 'Run Out': planning/behavior_velocity_run_out_module + - 'Occlusion Spot': planning/autoware_behavior_velocity_occlusion_spot_module + - 'Run Out': planning/autoware_behavior_velocity_run_out_module - 'Speed Bump': planning/behavior_velocity_speed_bump_module - - 'Stop Line': planning/behavior_velocity_stop_line_module - - 'Traffic Light': planning/behavior_velocity_traffic_light_module + - 'Stop Line': planning/autoware_behavior_velocity_stop_line_module + - 'Traffic Light': planning/autoware_behavior_velocity_traffic_light_module - 'Virtual Traffic Light': planning/autoware_behavior_velocity_virtual_traffic_light_module - - 'Walkway': planning/behavior_velocity_walkway_module + - 'Walkway': planning/autoware_behavior_velocity_walkway_module - 'Parking': - 'Freespace Planner': - 'About Freespace Planner': planning/freespace_planner @@ -48,36 +47,36 @@ nav: - 'Model Predictive Trajectory (MPT)': planning/autoware_path_optimizer/docs/mpt - 'How to Debug': planning/autoware_path_optimizer/docs/debug - 'Obstacle Cruise Planner': - - 'About Obstacle Cruise': planning/obstacle_cruise_planner - - 'How to Debug': planning/obstacle_cruise_planner/docs/debug + - 'About Obstacle Cruise': planning/autoware_obstacle_cruise_planner + - 'How to Debug': planning/autoware_obstacle_cruise_planner/docs/debug - 'Obstacle Stop Planner': planning/obstacle_stop_planner - - 'Obstacle Velocity Limiter': planning/obstacle_velocity_limiter - 'Path Smoother': - - 'About Path Smoother': planning/path_smoother - - 'Elastic Band': planning/path_smoother/docs/eb + - 'About Path Smoother': planning/autoware_path_smoother + - 'Elastic Band': planning/autoware_path_smoother/docs/eb - 'Sampling Based Planner': - - 'Path Sample': planning/sampling_based_planner/path_sampler - - 'Common library': planning/sampling_based_planner/sampler_common - - 'Frenet Planner': planning/sampling_based_planner/frenet_planner - - 'Bezier Sampler': planning/sampling_based_planner/bezier_sampler + - 'Path Sample': planning/sampling_based_planner/autoware_path_sampler + - 'Common library': planning/sampling_based_planner/autoware_sampler_common + - 'Frenet Planner': planning/sampling_based_planner/autoware_frenet_planner + - 'Bezier Sampler': planning/sampling_based_planner/autoware_bezier_sampler - 'Surround Obstacle Checker': - - 'About Surround Obstacle Checker': planning/surround_obstacle_checker - - 'About Surround Obstacle Checker (Japanese)': planning/surround_obstacle_checker/surround_obstacle_checker-design.ja + - 'About Surround Obstacle Checker': planning/autoware_surround_obstacle_checker + - 'About Surround Obstacle Checker (Japanese)': planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja - 'Motion Velocity Planner': - - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ + - 'About Motion Velocity Planner': planning/motion_velocity_planner/autoware_motion_velocity_planner_node/ - 'Available Modules': - - 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/ + - 'Out of Lane': planning/motion_velocity_planner/autoware_motion_velocity_planner_out_of_lane_module/ + - 'Obstacle Velocity Limiter': planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/ - 'Velocity Smoother': - 'About Velocity Smoother': planning/autoware_velocity_smoother - 'About Velocity Smoother (Japanese)': planning/autoware_velocity_smoother/README.ja - - 'Scenario Selector': planning/scenario_selector + - 'Scenario Selector': planning/autoware_scenario_selector - 'Static Centerline Generator': planning/autoware_static_centerline_generator - 'API and Library': - 'Costmap Generator': planning/costmap_generator - - 'External Velocity Limit Selector': planning/external_velocity_limit_selector - - 'Objects of Interest Marker Interface': planning/objects_of_interest_marker_interface - - 'Route Handler': planning/route_handler - - 'RTC Interface': planning/rtc_interface + - 'External Velocity Limit Selector': planning/autoware_external_velocity_limit_selector + - 'Objects of Interest Marker Interface': planning/autoware_objects_of_interest_marker_interface + - 'Route Handler': planning/autoware_route_handler + - 'RTC Interface': planning/autoware_rtc_interface - 'Additional Tools': - 'Remaining Distance Time Calculator': planning/autoware_remaining_distance_time_calculator - 'RTC Replayer': planning/rtc_replayer @@ -86,5 +85,5 @@ nav: - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md - 'Planning Test Utils': planning/planning_test_utils - 'Planning Test Manager': planning/autoware_planning_test_manager - - 'Planning Topic Converter': planning/planning_topic_converter - - 'Planning Validator': planning/planning_validator + - 'Planning Topic Converter': planning/autoware_planning_topic_converter + - 'Planning Validator': planning/autoware_planning_validator diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/CMakeLists.txt b/planning/autoware_behavior_path_avoidance_by_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..9cb5f7f5a9b03 --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_path_avoidance_by_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/manager.cpp + src/interface.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/README.md b/planning/autoware_behavior_path_avoidance_by_lane_change_module/README.md new file mode 100644 index 0000000000000..b7869fd4911a6 --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/README.md @@ -0,0 +1,40 @@ +# Avoidance by lane change design + +This is a sub-module to avoid obstacles by lane change maneuver. + +## Purpose / Role + +This module is designed as one of the obstacle avoidance features and generates a lane change path if the following conditions are satisfied. + +- Exist lane changeable lanelet. +- Exist avoidance target objects on ego driving lane. + +![avoidance_by_lane_change](./images/avoidance_by_lane_change.svg) + +## Inner-workings / Algorithms + +Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Static Object Avoidance Module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../autoware_behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. + +Check that the following conditions are satisfied after the filtering process for the avoidance target. + +### Number of the avoidance target objects + +This module is launched when the number of avoidance target objects on **EGO DRIVING LANE** is greater than `execute_object_num`. If there are no avoidance targets in the ego driving lane or their number is less than the parameter, the obstacle is avoided by normal avoidance behavior (if the normal avoidance module is registered). + +![trigger_1](./images/avoidance_by_lc_trigger_1.svg) + +### Lane change end point condition + +Unlike the normal avoidance module, which specifies the shift line end point, this module does not specify its end point when generating a lane change path. On the other hand, setting `execute_only_when_lane_change_finish_before_object` to `true` will activate this module only if the lane change can be completed before the avoidance target object. + +Although setting the parameter to `false` would increase the scene of avoidance by lane change, it is assumed that sufficient lateral margin may not be ensured in some cases because the vehicle passes by the side of obstacles during the lane change. + +![trigger_2](./images/avoidance_by_lc_trigger_2.svg) + +## Parameters + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------- | ---- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| execute_object_num | [-] | int | Number of avoidance target objects on ego driving lane is greater than this value, this module will be launched. | 1 | +| execute_object_longitudinal_margin | [m] | double | [maybe unused] Only when distance between the ego and avoidance target object is longer than this value, this module will be launched. | 0.0 | +| execute_only_when_lane_change_finish_before_object | [-] | bool | If this flag set `true`, this module will be launched only when the lane change end point is **NOT** behind the avoidance target object. | true | diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml b/planning/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml diff --git a/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg b/planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg diff --git a/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg b/planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg diff --git a/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg b/planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/package.xml b/planning/autoware_behavior_path_avoidance_by_lane_change_module/package.xml new file mode 100644 index 0000000000000..8bb9e96c881c1 --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/package.xml @@ -0,0 +1,41 @@ + + + + autoware_behavior_path_avoidance_by_lane_change_module + 0.1.0 + The behavior_path_avoidance_by_lane_change_module package + + Satoshi Ota + Zulfaqar Azmi + Fumiya Watanabe + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_path_lane_change_module + autoware_behavior_path_planner + autoware_behavior_path_planner_common + autoware_behavior_path_static_obstacle_avoidance_module + autoware_rtc_interface + lanelet2_extension + motion_utils + pluginlib + rclcpp + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/plugins.xml b/planning/autoware_behavior_path_avoidance_by_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..72c72c513b6da --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp new file mode 100644 index 0000000000000..cd515fbfce5da --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp @@ -0,0 +1,37 @@ +// 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 DATA_STRUCTS_HPP_ +#define DATA_STRUCTS_HPP_ + +#include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::AvoidanceParameters; + +struct AvoidanceByLCParameters : public AvoidanceParameters +{ + // execute only when the target object longitudinal distance is larger than this param. + double execute_object_longitudinal_margin{0.0}; + + // execute only when lane change end point is before the object. + bool execute_only_when_lane_change_finish_before_object{false}; + + explicit AvoidanceByLCParameters(const AvoidanceParameters & param) : AvoidanceParameters(param) + { + } +}; +} // namespace autoware::behavior_path_planner + +#endif // DATA_STRUCTS_HPP_ diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp new file mode 100644 index 0000000000000..c2e31dd6136f0 --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -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. + +#include "interface.hpp" + +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" + +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::State; +using autoware::route_handler::Direction; + +AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: LaneChangeInterface{ + name, + node, + parameters, + rtc_interface_ptr_map, + objects_of_interest_marker_interface_ptr_map, + std::make_unique(parameters, avoidance_by_lane_change_parameters)} +{ +} + +bool AvoidanceByLaneChangeInterface::isExecutionRequested() const +{ + return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() && + module_type_->isValidPath(); +} +void AvoidanceByLaneChangeInterface::processOnEntry() +{ + waitApproval(); +} + +void AvoidanceByLaneChangeInterface::updateRTCStatus( + const double start_distance, const double finish_distance) +{ + const auto direction = std::invoke([&]() -> std::string { + const auto dir = module_type_->getDirection(); + return (dir == Direction::LEFT) ? "left" : "right"; + }); + + const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING; + + rtc_interface_ptr_map_.at(direction)->updateCooperateStatus( + uuid_map_.at(direction), isExecutionReady(), state, start_distance, finish_distance, + clock_->now()); +} +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp new file mode 100644 index 0000000000000..77bcf3ec6e1aa --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp @@ -0,0 +1,54 @@ +// 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 INTERFACE_HPP_ +#define INTERFACE_HPP_ + +#include "autoware_behavior_path_lane_change_module/interface.hpp" +#include "data_structs.hpp" +#include "scene.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::LaneChangeInterface; +using autoware::behavior_path_planner::ObjectsOfInterestMarkerInterface; +using autoware::behavior_path_planner::RTCInterface; + +class AvoidanceByLaneChangeInterface : public LaneChangeInterface +{ +public: + AvoidanceByLaneChangeInterface( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::shared_ptr & avoidance_by_lane_change_parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); + + bool isExecutionRequested() const override; + + void processOnEntry() override; + +protected: + void updateRTCStatus(const double start_distance, const double finish_distance) override; +}; +} // namespace autoware::behavior_path_planner + +#endif // INTERFACE_HPP_ diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp new file mode 100644 index 0000000000000..867187ce5055a --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -0,0 +1,201 @@ +// 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 "autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" +#include "data_structs.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::getParameter; +using autoware::behavior_path_planner::ObjectParameter; + +void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) +{ + using autoware_perception_msgs::msg::ObjectClassification; + using tier4_autoware_utils::getOrDeclareParameter; + + // init manager interface + initInterface(node, {"left", "right"}); + + // init lane change manager + LaneChangeModuleManager::initParams(node); + + const auto avoidance_params = getParameter(node); + AvoidanceByLCParameters p(avoidance_params); + + // unique parameters + { + const std::string ns = "avoidance_by_lane_change."; + p.execute_object_longitudinal_margin = + getOrDeclareParameter(*node, ns + "execute_object_longitudinal_margin"); + p.execute_only_when_lane_change_finish_before_object = + getOrDeclareParameter(*node, ns + "execute_only_when_lane_change_finish_before_object"); + } + + // general params + { + const std::string ns = "avoidance."; + p.resample_interval_for_planning = + getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); + p.resample_interval_for_output = + getOrDeclareParameter(*node, ns + "resample_interval_for_output"); + } + + // target object + { + const auto get_object_param = [&](std::string && ns) { + ObjectParameter param{}; + param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); + param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "th_moving_speed"); + param.moving_time_threshold = getOrDeclareParameter(*node, ns + "th_moving_time"); + param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); + param.envelope_buffer_margin = + getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); + param.lateral_soft_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + param.lateral_hard_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); + return param; + }; + + const std::string ns = "avoidance_by_lane_change.target_object."; + p.object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p.object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p.object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p.object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p.object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p.object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p.object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p.object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + + p.lower_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "lower_distance_for_polygon_expansion"); + p.upper_distance_for_polygon_expansion = + getOrDeclareParameter(*node, ns + "upper_distance_for_polygon_expansion"); + } + + // target filtering + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + p.object_check_goal_distance = + getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "max_compensation_time"); + } + + { + const std::string ns = "avoidance.target_filtering.parked_vehicle."; + p.threshold_distance_object_is_on_center = + getOrDeclareParameter(*node, ns + "th_offset_from_centerline"); + p.object_check_shiftable_ratio = + getOrDeclareParameter(*node, ns + "th_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + getOrDeclareParameter(*node, ns + "min_road_shoulder_width"); + } + + { + const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.time_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.th_moving_distance"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + + // avoidance maneuver (longitudinal) + { + const std::string ns = "avoidance.avoidance.longitudinal."; + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); + p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_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"); + } + + { + const 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 + { + const std::string ns = "avoidance.safety_check."; + p.hysteresis_factor_expand_rate = + getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); + } + + avoidance_parameters_ = std::make_shared(p); +} + +std::unique_ptr +AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); +} + +} // namespace autoware::behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager, + autoware::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp new file mode 100644 index 0000000000000..2a282c42d1760 --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp @@ -0,0 +1,54 @@ +// 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 "autoware_behavior_path_lane_change_module/manager.hpp" +#include "data_structs.hpp" +#include "interface.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::LaneChangeModuleManager; +using autoware::behavior_path_planner::LaneChangeModuleType; +using autoware::behavior_path_planner::SceneModuleInterface; + +class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager +{ +public: + AvoidanceByLaneChangeModuleManager() + : LaneChangeModuleManager( + "avoidance_by_lane_change", autoware::route_handler::Direction::NONE, + LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) + { + } + + void init(rclcpp::Node * node) override; + + std::unique_ptr createNewSceneModuleInstance() override; + +private: + std::shared_ptr avoidance_parameters_; +}; +} // namespace autoware::behavior_path_planner + +#endif // MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp new file mode 100644 index 0000000000000..ac3904c060f3a --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -0,0 +1,302 @@ +// 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 "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::Direction; +using autoware::behavior_path_planner::LaneChangeModuleType; +using autoware::behavior_path_planner::ObjectInfo; +using autoware::behavior_path_planner::Point2d; +using autoware::behavior_path_planner::utils::lane_change::debug::createExecutionArea; +namespace utils = autoware::behavior_path_planner::utils; + +AvoidanceByLaneChange::AvoidanceByLaneChange( + const std::shared_ptr & parameters, + std::shared_ptr avoidance_parameters) +: NormalLaneChange(parameters, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE, Direction::NONE), + avoidance_parameters_(std::move(avoidance_parameters)), + avoidance_helper_{std::make_shared(avoidance_parameters_)} +{ +} + +bool AvoidanceByLaneChange::specialRequiredCheck() const +{ + const auto & data = avoidance_data_; + + if (data.target_objects.empty()) { + RCLCPP_DEBUG(logger_, "no empty objects"); + return false; + } + + const auto & object_parameters = avoidance_parameters_->object_parameters; + + const auto count_target_object = [&](const auto sum, const auto & p) { + const auto & objects = avoidance_data_.target_objects; + + const auto is_avoidance_target = [&p](const auto & object) { + const auto target_class = utils::getHighestProbLabel(object.object.classification) == p.first; + return target_class && object.avoid_required; + }; + + return sum + std::count_if(objects.begin(), objects.end(), is_avoidance_target); + }; + const auto num_of_avoidance_targets = + std::accumulate(object_parameters.begin(), object_parameters.end(), 0UL, count_target_object); + + if (num_of_avoidance_targets < 1) { + RCLCPP_DEBUG(logger_, "no avoidance target"); + return false; + } + + const auto & nearest_object = data.target_objects.front(); + const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); + const auto minimum_lane_change_length = calcMinimumLaneChangeLength(); + + lane_change_debug_.execution_area = createExecutionArea( + getCommonParam().vehicle_info, getEgoPose(), + std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); + + RCLCPP_DEBUG( + logger_, "Conditions ? %f, %f, %f", nearest_object.longitudinal, minimum_lane_change_length, + minimum_avoid_length); + return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_length); +} + +bool AvoidanceByLaneChange::specialExpiredCheck() const +{ + return !specialRequiredCheck(); +} + +void AvoidanceByLaneChange::updateSpecialData() +{ + const auto p = std::dynamic_pointer_cast(avoidance_parameters_); + + avoidance_debug_data_ = DebugData(); + avoidance_data_ = calcAvoidancePlanningData(avoidance_debug_data_); + + if (avoidance_data_.target_objects.empty()) { + direction_ = Direction::NONE; + } else { + direction_ = utils::static_obstacle_avoidance::isOnRight(avoidance_data_.target_objects.front()) + ? Direction::LEFT + : Direction::RIGHT; + } + + utils::static_obstacle_avoidance::updateRegisteredObject( + registered_objects_, avoidance_data_.target_objects, p); + utils::static_obstacle_avoidance::compensateDetectionLost( + registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects); + + std::sort( + avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(), + [](auto a, auto b) { return a.longitudinal < b.longitudinal; }); +} + +AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( + AvoidanceDebugData & debug) const +{ + AvoidancePlanningData data; + + // reference pose + data.reference_pose = getEgoPose(); + + data.reference_path_rough = prev_module_output_.path; + + const auto resample_interval = avoidance_parameters_->resample_interval_for_planning; + data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval); + + data.current_lanelets = getCurrentLanes(); + + fillAvoidanceTargetObjects(data, debug); + + return data; +} + +void AvoidanceByLaneChange::fillAvoidanceTargetObjects( + AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const +{ + const auto p = std::dynamic_pointer_cast(avoidance_parameters_); + + const auto [object_within_target_lane, object_outside_target_lane] = + utils::path_safety_checker::separateObjectsByLanelets( + *planner_data_->dynamic_object, data.current_lanelets, + [](const auto & obj, const auto & lane) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane); + }); + + // 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 + // data.target_objects is equal to object_within_target_lane + { + const auto other_objects_size = + object_within_target_lane.objects.size() + object_outside_target_lane.objects.size(); + data.other_objects.reserve(other_objects_size); + data.target_objects.reserve(object_within_target_lane.objects.size()); + } + + { + const auto & objects = object_outside_target_lane.objects; + std::transform( + objects.cbegin(), objects.cend(), std::back_inserter(data.other_objects), + [](const auto & object) { + ObjectData other_object; + other_object.object = object; + other_object.info = ObjectInfo::OUT_OF_TARGET_AREA; + return other_object; + }); + } + + ObjectDataArray target_lane_objects; + target_lane_objects.reserve(object_within_target_lane.objects.size()); + for (const auto & obj : object_within_target_lane.objects) { + const auto target_lane_object = createObjectData(data, obj); + if (!target_lane_object) { + continue; + } + + target_lane_objects.push_back(*target_lane_object); + } + + data.target_objects = target_lane_objects; +} + +std::optional AvoidanceByLaneChange::createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const +{ + using boost::geometry::return_centroid; + using motion_utils::findNearestIndex; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::calcLateralDeviation; + + const auto p = std::dynamic_pointer_cast(avoidance_parameters_); + + const auto & path_points = data.reference_path.points; + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = findNearestIndex(path_points, object_pose.position); + const auto object_closest_pose = path_points.at(object_closest_index).point.pose; + const auto t = utils::getHighestProbLabel(object.classification); + const auto & object_parameter = avoidance_parameters_->object_parameters.at(t); + + ObjectData object_data{}; + // Calc lateral deviation from path to target object. + object_data.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + + if ( + std::abs(object_data.to_centerline) < + avoidance_parameters_->threshold_distance_object_is_on_center) { + return std::nullopt; + } + + object_data.object = object; + + const auto lower = p->lower_distance_for_polygon_expansion; + const auto upper = p->upper_distance_for_polygon_expansion; + const auto clamp = + std::clamp(calcDistance2d(getEgoPose(), object_pose) - lower, 0.0, upper) / upper; + object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0; + + // Calc envelop polygon. + utils::static_obstacle_avoidance::fillObjectEnvelopePolygon( + object_data, registered_objects_, object_closest_pose, p); + + // calc object centroid. + object_data.centroid = return_centroid(object_data.envelope_poly); + + // Calc moving time. + utils::static_obstacle_avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); + + object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 + ? Direction::LEFT + : Direction::RIGHT; + + // Find the footprint point closest to the path, set to object_data.overhang_distance. + object_data.overhang_points = utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance( + object_data, data.reference_path); + + // Check whether the the ego should avoid the object. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; + utils::static_obstacle_avoidance::fillAvoidanceNecessity( + object_data, registered_objects_, vehicle_width, p); + + utils::static_obstacle_avoidance::fillLongitudinalAndLengthByClosestEnvelopeFootprint( + data.reference_path_rough, getEgoPosition(), object_data); + return object_data; +} + +double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_object) const +{ + const auto ego_width = getCommonParam().vehicle_width; + const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); + const auto nearest_object_parameter = + avoidance_parameters_->object_parameters.at(nearest_object_type); + const auto lateral_hard_margin = std::max( + nearest_object_parameter.lateral_hard_margin, + nearest_object_parameter.lateral_hard_margin_for_parked_vehicle); + const auto avoid_margin = lateral_hard_margin * nearest_object.distance_factor + + nearest_object_parameter.lateral_soft_margin + 0.5 * ego_width; + + avoidance_helper_->setData(planner_data_); + const auto shift_length = avoidance_helper_->getShiftLength( + nearest_object, utils::static_obstacle_avoidance::isOnRight(nearest_object), avoid_margin); + + return avoidance_helper_->getMinAvoidanceDistance(shift_length); +} + +double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const +{ + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_DEBUG(logger_, "no empty lanes"); + return std::numeric_limits::infinity(); + } + + return utils::lane_change::calcMinimumLaneChangeLength( + getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_); +} + +double AvoidanceByLaneChange::calcLateralOffset() const +{ + auto additional_lat_offset{0.0}; + for (const auto & [type, p] : avoidance_parameters_->object_parameters) { + const auto lateral_hard_margin = + std::max(p.lateral_hard_margin, p.lateral_hard_margin_for_parked_vehicle); + const auto offset = + 2.0 * p.envelope_buffer_margin + lateral_hard_margin + p.lateral_soft_margin; + additional_lat_offset = std::max(additional_lat_offset, offset); + } + return additional_lat_offset; +} +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp new file mode 100644 index 0000000000000..bce49f8299083 --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp @@ -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. + +#ifndef SCENE_HPP_ +#define SCENE_HPP_ + +#include "autoware_behavior_path_lane_change_module/scene.hpp" +#include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" +#include "data_structs.hpp" + +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::DebugData; +using AvoidanceDebugData = DebugData; +using autoware::behavior_path_planner::AvoidancePlanningData; +using autoware::behavior_path_planner::LaneChangeParameters; +using autoware::behavior_path_planner::NormalLaneChange; +using autoware::behavior_path_planner::ObjectData; +using autoware::behavior_path_planner::ObjectDataArray; +using autoware::behavior_path_planner::PredictedObject; +using autoware::behavior_path_planner::helper::static_obstacle_avoidance::AvoidanceHelper; + +class AvoidanceByLaneChange : public NormalLaneChange +{ +public: + AvoidanceByLaneChange( + const std::shared_ptr & parameters, + std::shared_ptr avoidance_by_lane_change_parameters); + + bool specialRequiredCheck() const override; + + bool specialExpiredCheck() const override; + + void updateSpecialData() override; + +private: + std::shared_ptr avoidance_parameters_; + + AvoidancePlanningData calcAvoidancePlanningData(AvoidanceDebugData & debug) const; + AvoidancePlanningData avoidance_data_; + mutable AvoidanceDebugData avoidance_debug_data_; + + ObjectDataArray registered_objects_; + mutable ObjectDataArray stopped_objects_; + std::shared_ptr avoidance_helper_; + + std::optional createObjectData( + const AvoidancePlanningData & data, const PredictedObject & object) const; + + void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const; + + double calcMinAvoidanceLength(const ObjectData & nearest_object) const; + double calcMinimumLaneChangeLength() const; + double calcLateralOffset() const; +}; +} // namespace autoware::behavior_path_planner + +#endif // SCENE_HPP_ diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..561724f4a6a99 --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,132 @@ +// 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 "autoware_behavior_path_planner/behavior_path_planner_node.hpp" + +#include +#include +#include + +#include + +#include +#include + +using autoware::behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", + ament_index_cpp::get_package_share_directory( + "autoware_behavior_path_static_obstacle_avoidance_module") + + "/config/static_obstacle_avoidance.param.yaml", + ament_index_cpp::get_package_share_directory( + "autoware_behavior_path_avoidance_by_lane_change_module") + + "/config/avoidance_by_lane_change.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt index e7df28307e754..c295b71537781 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(autoware_behavior_path_dynamic_obstacle_avoidance_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index caf0544b0852e..f42956c60bbce 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -16,7 +16,7 @@ #define AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ #include "autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -25,7 +25,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface @@ -53,6 +53,6 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface std::shared_ptr parameters_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #endif // AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 1d6081d3ac62a..07cea08d491f9 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ #define AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include @@ -55,7 +55,7 @@ std::vector getAllKeys(const std::unordered_map & map) } } // namespace -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using autoware_perception_msgs::msg::PredictedPath; using tier4_autoware_utils::Polygon2d; @@ -459,6 +459,6 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #endif // AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 33db04f3c8e3c..a22e34f20a922 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -16,11 +16,11 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_path_planner + autoware_behavior_path_planner_common autoware_perception_msgs autoware_planning_msgs autoware_vehicle_msgs - behavior_path_planner - behavior_path_planner_common geometry_msgs lanelet2_core lanelet2_extension diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml index 3827012b12e22..598e6a70b7f5f 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp index de524a3b63de9..cc87e9f1c3c81 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/manager.cpp @@ -22,7 +22,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { namespace { @@ -291,9 +291,9 @@ bool DynamicObstacleAvoidanceModuleManager::isAlwaysExecutableModule() const { return true; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::DynamicObstacleAvoidanceModuleManager, - behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::DynamicObstacleAvoidanceModuleManager, + autoware::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index b2c5d77402d9a..0cf9b0fc7bd8b 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" @@ -37,7 +37,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { namespace { @@ -1861,4 +1861,4 @@ DynamicObstacleAvoidanceModule::calcEgoPathReservePoly(const PathWithLaneId & eg return {left_avoid_poly, right_avoid_poly}; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index bd9e6fd483c7c..91b0f4c9df9f5 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" #include #include @@ -20,7 +20,7 @@ #include -using behavior_path_planner::BehaviorPathPlannerNode; +using autoware::behavior_path_planner::BehaviorPathPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -47,7 +47,7 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); + module_names.emplace_back("autoware::behavior_path_planner::DynamicAvoidanceModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt b/planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt index 662d36618e766..521a125dec2c2 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(autoware_behavior_path_external_request_lane_change_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/manager.cpp diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/autoware_behavior_path_external_request_lane_change_module/package.xml index c08eb20a2b589..b794789bd19e0 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -18,13 +18,13 @@ autoware_cmake eigen3_cmake_module - behavior_path_lane_change_module - behavior_path_planner - behavior_path_planner_common + autoware_behavior_path_lane_change_module + autoware_behavior_path_planner + autoware_behavior_path_planner_common + autoware_rtc_interface motion_utils pluginlib rclcpp - rtc_interface tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml b/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml index d13dff53e0836..9cc9862590965 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml +++ b/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml @@ -1,4 +1,4 @@ - - + + diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index 130cbff29bcf3..904fbc1f0c4ae 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -14,12 +14,12 @@ #include "manager.hpp" -#include "behavior_path_lane_change_module/interface.hpp" +#include "autoware_behavior_path_lane_change_module/interface.hpp" #include "scene.hpp" namespace autoware::behavior_path_planner { -using ::behavior_path_planner::LaneChangeInterface; +using autoware::behavior_path_planner::LaneChangeInterface; std::unique_ptr ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() @@ -44,7 +44,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, - ::behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::SceneModuleManagerInterface) PLUGINLIB_EXPORT_CLASS( autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, - ::behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp index a4f3dce188c73..7cfce807cd663 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp @@ -15,8 +15,8 @@ #ifndef MANAGER_HPP_ #define MANAGER_HPP_ -#include "behavior_path_lane_change_module/manager.hpp" -#include "route_handler/route_handler.hpp" +#include "autoware_behavior_path_lane_change_module/manager.hpp" +#include "autoware_route_handler/route_handler.hpp" #include @@ -25,9 +25,9 @@ namespace autoware::behavior_path_planner { -using ::behavior_path_planner::LaneChangeModuleManager; -using ::behavior_path_planner::LaneChangeModuleType; -using ::behavior_path_planner::SceneModuleInterface; +using autoware::behavior_path_planner::LaneChangeModuleManager; +using autoware::behavior_path_planner::LaneChangeModuleType; +using autoware::behavior_path_planner::SceneModuleInterface; class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager { diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp index 2ba5a5aea34d4..ce2a75d0eebce 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp @@ -20,7 +20,7 @@ namespace autoware::behavior_path_planner { -using ::behavior_path_planner::LaneChangeModuleType; +using autoware::behavior_path_planner::LaneChangeModuleType; ExternalRequestLaneChange::ExternalRequestLaneChange( const std::shared_ptr & parameters, Direction direction) diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp index d2eb20b048a5d..bc26688865798 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp @@ -15,15 +15,15 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include "behavior_path_lane_change_module/scene.hpp" +#include "autoware_behavior_path_lane_change_module/scene.hpp" #include namespace autoware::behavior_path_planner { -using ::behavior_path_planner::Direction; -using ::behavior_path_planner::LaneChangeParameters; -using ::behavior_path_planner::NormalLaneChange; +using autoware::behavior_path_planner::Direction; +using autoware::behavior_path_planner::LaneChangeParameters; +using autoware::behavior_path_planner::NormalLaneChange; class ExternalRequestLaneChange : public NormalLaneChange { diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 201d01e9b7fa0..642e4d7fc6a30 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" #include "autoware_planning_test_manager/autoware_planning_test_manager.hpp" -#include "behavior_path_planner/behavior_path_planner_node.hpp" #include "planning_test_utils/planning_test_utils.hpp" #include @@ -22,7 +22,7 @@ #include #include -using ::behavior_path_planner::BehaviorPathPlannerNode; +using autoware::behavior_path_planner::BehaviorPathPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -48,7 +48,7 @@ std::shared_ptr generateNode() const auto behavior_path_planner_dir = ament_index_cpp::get_package_share_directory("behavior_path_planner"); const auto behavior_path_lane_change_module_dir = - ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); + ament_index_cpp::get_package_share_directory("autoware_behavior_path_lane_change_module"); std::vector module_names; module_names.emplace_back( diff --git a/planning/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/autoware_behavior_path_goal_planner_module/CMakeLists.txt new file mode 100644 index 0000000000000..670a61479e875 --- /dev/null +++ b/planning/autoware_behavior_path_goal_planner_module/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_path_goal_planner_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/default_fixed_goal_planner.cpp + src/freespace_pull_over.cpp + src/geometric_pull_over.cpp + src/goal_searcher.cpp + src/shift_pull_over.cpp + src/util.cpp + src/goal_planner_module.cpp + src/manager.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/autoware_behavior_path_goal_planner_module/README.md b/planning/autoware_behavior_path_goal_planner_module/README.md new file mode 100644 index 0000000000000..7cca725fecca5 --- /dev/null +++ b/planning/autoware_behavior_path_goal_planner_module/README.md @@ -0,0 +1,429 @@ +# Goal Planner design + +## Purpose / Role + +Plan path around the goal. + +- Arrive at the designated goal. +- Modify the goal to avoid obstacles or to pull over at the side of the lane. + +## Design + +If goal modification is not allowed, park at the designated fixed goal. (`fixed_goal_planner` in the figure below) +When allowed, park in accordance with the specified policy(e.g pull over on left/right side of the lane). (`rough_goal_planner` in the figure below). Currently rough goal planner only support pull_over feature, but it would be desirable to be able to accommodate various parking policies in the future. + +```plantuml +@startuml +package goal_planner{ + + class GoalPlannerModule {} + + package rough_goal_planner <>{ + + package lane_parking <>{ + class ShiftPullOver {} + class GeometricPullOver {} + } + + package freespace_parking <>{ + class FreeSpacePullOver {} + } + + class GoalSearcher {} + + struct GoalCandidates {} + struct PullOverPath{} + + abstract class PullOverPlannerBase {} + abstract class GoalsearcherBase {} + + } + + package fixed_goal_planner <>{ + abstract class FixedGoalPlannerBase {} + class DefaultFixedPlanner{} + } +} + + +package utils{ + class PathShifter {} + + class GeometricParallelParking {} +} + +package freespace_planning_algorithms + { + class AstarSearch{} + class RRTStar{} +} + +' goal planner +ShiftPullOver --|> PullOverPlannerBase +GeometricPullOver --|> PullOverPlannerBase +FreeSpacePullOver --|> PullOverPlannerBase +GoalSearcher --|> GoalSearcherBase +DefaultFixedPlanner --|> FixedGoalPlannerBase + +PathShifter --o ShiftPullOver +GeometricParallelParking --o GeometricPullOver +AstarSearch --o FreeSpacePullOver +RRTStar --o FreeSpacePullOver + +PullOverPlannerBase --o GoalPlannerModule +GoalSearcherBase --o GoalPlannerModule +FixedGoalPlannerBase --o GoalPlannerModule + +PullOverPath --o PullOverPlannerBase +GoalCandidates --o GoalSearcherBase + +@enduml +``` + +## start condition + +### fixed_goal_planner + +This is a very simple function that plans a smooth path to a specified goal. This function does not require approval and always runs with the other modules. +_NOTE: this planner does not perform the several features described below, such as "goal search", "collision check", "safety check", etc._ + +Executed when both conditions are met. + +- Route is set with `allow_goal_modification=false`. This is the default. +- The goal is set in the normal lane. In other words, it is NOT `road_shoulder`. +- Ego-vehicle exists in the same lane sequence as the goal. + +If the target path contains a goal, modify the points of the path so that the path and the goal are connected smoothly. This process will change the shape of the path by the distance of `refine_goal_search_radius_range` from the goal. Note that this logic depends on the interpolation algorithm that will be executed in a later module (at the moment it uses spline interpolation), so it needs to be updated in the future. + +![path_goal_refinement](./images/path_goal_refinement.drawio.svg) + + + +### rough_goal_planner + +#### pull over on road lane + +- 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. +- ego-vehicle is in the same lane as the goal. + + + +#### pull over on shoulder lane + +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. +- Goal is set in the `road_shoulder`. + + + +## finish condition + +- The distance to the goal from your vehicle is lower than threshold (default: < `1m`). +- The ego-vehicle is stopped. + - The speed is lower than threshold (default: < `0.01m/s`). + +## General parameters for goal_planner + +| 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 | + +## **Goal Search** + +To realize pull over even when an obstacle exists near the original goal, a collision free area is searched within a certain range around the original goal. The goal found will be published as `/planning/scenario_planning/modified_goal`. + +[goal search video](https://user-images.githubusercontent.com/39142679/188359594-c6724e3e-1cb7-4051-9a18-8d2c67d4dee9.mp4) + +1. The original goal is set, and the refined goal pose is obtained by moving in the direction normal to the lane center line and keeping `margin_from_boundary` from the edge of the lane. + ![refined_goal](./images/goal_planner-refined_goal.drawio.svg) + +2. Using `refined_goal` as the base goal, search for candidate goals in the range of `-forward_goal_search_length` to `backward_goal_search_length` in the longitudinal direction and `longitudinal_margin` to `longitudinal_margin+max_lateral_offset` in th lateral direction based on refined_goal. + ![goal_candidates](./images/goal_planner-goal_candidates.drawio.svg) + +3. Each candidate goal is prioritized and a path is generated for each planner for each goal. The priority of a candidate goal is determined by its distance from the base goal. The ego vehicle tries to park for the highest possible goal. The distance is determined by the selected policy. In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance. This means the distance is calculated by `longitudinal_distance + lateral_cost*lateral_distance` + ![goal_distance](./images/goal_planner-goal_distance.drawio.svg) + The following figure is an example of minimum_weighted_distance.​ The white number indicates the goal candidate priority, and the smaller the number, the higher the priority. the 0 goal indicates the base goal. + ![goal_priority_rviz_with_goal](./images/goal_priority_with_goal.png) + ![goal_priority_rviz](./images/goal_priority_rviz.png) + +4. If the footprint in each goal candidate is within `object_recognition_collision_check_margin` of that of the object, it is determined to be unsafe. These goals are not selected. If `use_occupancy_grid_for_goal_search` is enabled, collision detection on the grid is also performed with `occupancy_grid_collision_check_margin`. + +Red goals candidates in the image indicate unsafe ones. + +![is_safe](./images/goal_planner-is_safe.drawio.svg) + +It is possible to keep `longitudinal_margin` in the longitudinal direction apart from the collision margin for obstacles from the goal candidate. This is intended to provide natural spacing for parking and efficient departure. + +![longitudinal_margin](./images/goal_planner-longitudinal_margin.drawio.svg) + +Also, if `prioritize_goals_before_objects` is enabled, To arrive at each goal, the number of objects that need to be avoided in the target range is counted, and those with the lowest number are given priority. + +The images represent a count of objects to be avoided at each range, with priority given to those with the lowest number, regardless of the aforementioned distances. + +![object_to_avoid](./images/goal_planner-object_to_avoid.drawio.svg) + +The gray numbers represent objects to avoid, and you can see that the goal in front has a higher priority in this case. + +![goal_priority_object_to_avoid_rviz.png](./images/goal_priority_object_to_avoid_rviz.png) + +### Parameters for goal search + +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_longitudinal_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_weighted_distance`, sort with the sum of weighted lateral distance and longitudinal distance | `minimum_weighted_distance` | +| lateral_weight | [-] | double | Weight for lateral distance used when `minimum_weighted_distance` | 40.0 | +| prioritize_goals_before_objects | [-] | bool | If there are objects that may need to be avoided, prioritize the goal in front of them | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | + +## **Pull Over** + +There are three path generation methods. +The path is generated with a certain margin (default: `0.75 m`) from the boundary of shoulder lane. + +The process is time consuming because multiple planners are used to generate path for each candidate goal. Therefore, in this module, the path generation is performed in a thread different from the main thread. +Path generation is performed at the timing when the shape of the output path of the previous module changes. If a new module launches, it is expected to go to the previous stage before the goal planner, in which case the goal planner re-generates the path. The goal planner is expected to run at the end of multiple modules, which is achieved by `keep_last` in the planner manager. + +Threads in the goal planner are shown below. + +![threads.png](./images/goal_planner-threads.drawio.svg) + +The main thread will be the one called from the planner manager flow. + +- The goal candidate generation and path candidate generation are done in a separate thread(lane path generation thread). +- The path candidates generated there are referred to by the main thread, and the one judged to be valid for the current planner data (e.g. ego and object information) is selected from among them. valid means no sudden deceleration, no collision with obstacles, etc. The selected path will be the output of this module. +- If there is no path selected, or if the selected path is collision and ego is stuck, a separate thread(freespace path generation thread) will generate a path using freespace planning algorithm. If a valid free space path is found, it will be the output of the module. If the object moves and the pull over path generated along the lane is collision-free, the path is used as output again. See also the section on freespace parking for more information on the flow of generating freespace paths. + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :----- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :--------------------------------------- | +| 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** + +Pull over distance is calculated by the speed, lateral deviation, and the lateral jerk. +The lateral jerk is searched for among the predetermined minimum and maximum values, and the one satisfies ready conditions described above is output. + +1. Apply uniform offset to centerline of shoulder lane for ensuring margin +2. In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) +3. Combine this path with center line of road lane + +![shift_parking](./images/shift_parking.drawio.svg) + +[shift_parking video](https://user-images.githubusercontent.com/39142679/178034101-4dc61a33-bc49-41a0-a9a8-755cce53cbc6.mp4) + +#### Parameters for shift parking + +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ | +| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | +| shift_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | +| after_shift_straight_distance | [m] | double | straight line distance after pull over end point | 1.0 | + +### **geometric parallel parking** + +Generate two arc paths with discontinuous curvature. It stops twice in the middle of the path to control the steer on the spot. There are two path generation methods: forward and backward. +See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm. There is also [a simple python implementation](https://github.com/kosuke55/geometric-parallel-parking). + +#### Parameters geometric parallel parking + +| Name | Unit | Type | Description | Default value | +| :---------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| arc_path_interval | [m] | double | interval between arc path points | 1.0 | +| pull_over_max_steer_rad | [rad] | double | maximum steer angle for path generation. it may not be possible to control steer up to max_steer_angle in vehicle_info when stopped | 0.35 | + +#### arc forward parking + +Generate two forward arc paths. + +![arc_forward_parking](./images/arc_forward_parking.drawio.svg) + +[arc_forward_parking video](https://user-images.githubusercontent.com/39142679/178034128-4754c401-8aff-4745-b69a-4a69ca29ce4b.mp4) + +#### Parameters arc forward parking + +| Name | Unit | Type | Description | Default value | +| :-------------------------------------- | :---- | :----- | :------------------------------------------------------------------------------ | :------------ | +| enable_arc_forward_parking | [-] | bool | flag whether to enable arc forward parking | true | +| after_forward_parking_straight_distance | [m] | double | straight line distance after pull over end point | 2.0 | +| forward_parking_velocity | [m/s] | double | velocity when forward parking | 1.38 | +| forward_parking_lane_departure_margin | [m/s] | double | lane departure margin for front left corner of ego-vehicle when forward parking | 0.0 | + +#### arc backward parking + +Generate two backward arc paths. + +![arc_backward_parking](./images/arc_backward_parking.drawio.svg). + +[arc_backward_parking video](https://user-images.githubusercontent.com/39142679/178034280-4b6754fe-3981-4aee-b5e0-970f34563c6d.mp4) + +#### Parameters arc backward parking + +| Name | Unit | Type | Description | Default value | +| :--------------------------------------- | :---- | :----- | :------------------------------------------------------------------------ | :------------ | +| enable_arc_backward_parking | [-] | bool | flag whether to enable arc backward parking | true | +| after_backward_parking_straight_distance | [m] | double | straight line distance after pull over end point | 2.0 | +| backward_parking_velocity | [m/s] | double | velocity when backward parking | -1.38 | +| backward_parking_lane_departure_margin | [m/s] | double | lane departure margin for front right corner of ego-vehicle when backward | 0.0 | + +### freespace parking + +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](./images/pull_over_freespace_parking_flowchart.drawio.svg) + +Simultaneous execution with `avoidance_module` in the flowchart is under development. + + + +#### Parameters freespace parking + +| Name | Unit | Type | Description | Default value | +| :----------------------- | :--- | :--- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_freespace_parking | [-] | bool | This flag enables freespace parking, which runs when the vehicle is stuck due to e.g. obstacles in the parking area. | true | + +See [freespace_planner](../freespace_planner/README.md) for other parameters. + +## **collision check for path generation** + +To select a safe one from the path candidates, a collision check with obstacles is performed. + +### **occupancy grid based collision check** + +Generate footprints from ego-vehicle path points and determine obstacle collision from the value of occupancy_grid of the corresponding cell. + +#### Parameters for occupancy grid based collision check + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | +| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | +| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | + +### **object recognition based collision check** + +A collision decision is made for each of the path candidates, and a collision-free path is selected. +There are three main margins at this point. + +- `object_recognition_collision_check_margin` is margin in all directions of ego. +- In the forward direction, a margin is added by the braking distance calculated from the current speed and maximum deceleration. The maximum distance is The maximum value of the distance is suppressed by the `object_recognition_collision_check_max_extra_stopping_margin` +- In curves, the lateral margin is larger than in straight lines.This is because curves are more prone to control errors or to fear when close to objects (The maximum value is limited by `object_recognition_collision_check_max_extra_stopping_margin`, although it has no basis.) + +![collision_check_margin](./images/goal_planner-collision_check_margin.drawio.svg) + +Then there is the concept of soft and hard margins. Although not currently parameterized, if a collision-free path can be generated by a margin several times larger than `object_recognition_collision_check_margin`, then the priority is higher. + +#### Parameters for object recognition based collision check + +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | :--- | :------------- | :------------------------------------------------------------------------------------------------------- | :-------------- | +| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | +| object_recognition_collision_check_soft_margins | [m] | vector[double] | soft margins for collision check when path generation | [2.0, 1.5, 1.0] | +| object_recognition_collision_check_hard_margins | [m] | vector[double] | hard margins for collision check when path generation | [0.6] | +| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | +| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | + +## **safety check** + +Perform safety checks on moving objects. If the object is determined to be dangerous, no path decision is made and no approval is given, + +- path decision is not made and approval is not granted. +- After approval, the ego vehicle stops under deceleration and jerk constraints. + +This module has two methods of safety check, `RSS` and `integral_predicted_polygon`. + +`RSS` method is a method commonly used by other behavior path planner modules, see [RSS based safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md). + +`integral_predicted_polygon` is a more safety-oriented method. This method is implemented because speeds during pull over are lower than during driving, and fewer objects travel along the edge of the lane. (It is sometimes too reactive and may be less available.) +This method integrates the footprints of egos and objects at a given time and checks for collisions between them. + +![safety_check](./images/goal_planner-safety_check.drawio.svg) + +In addition, the safety check has a time hysteresis, and if the path is judged "safe" for a certain period of time(`keep_unsafe_time`), it is finally treated as "safe". + +```txt + ==== is_safe + ---- current_is_safe + is_safe + ^ + | + | time + 1 +--+ +---+ +---========= +--+ + | | | | | | | | + | | | | | | | | + | | | | | | | | + | | | | | | | | + 0 =========================-------==========--> t +``` + +### Parameters for safety check + +| Name | Unit | Type | Description | Default value | +| :----------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------- | :--------------------------- | +| enable_safety_check | [-] | bool | flag whether to use safety check | true | +| method | [-] | string | method for safety check. `RSS` or `integral_predicted_polygon` | `integral_predicted_polygon` | +| keep_unsafe_time | [s] | double | safety check Hysteresis time. if the path is judged "safe" for the time it is finally treated as "safe". | 3.0 | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | + +#### Parameters for RSS safety check + +| Name | Unit | Type | Description | Default value | +| :---------------------------------- | :--- | :----- | :-------------------------------------- | :------------ | +| rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | + +#### Parameters for integral_predicted_polygon safety check + +| Name | Unit | Type | Description | Default value | +| :-------------- | :--- | :----- | :------------------------------------- | :------------ | +| forward_margin | [m] | double | forward margin for ego footprint | 1.0 | +| backward_margin | [m] | double | backward margin for ego footprint | 1.0 | +| lat_margin | [m] | double | lateral margin for ego footprint | 1.0 | +| time_horizon | [s] | double | Time width to integrate each footprint | 10.0 | + +## **path deciding** + +When `decide_path_distance` closer to the start of the pull over, if it is collision-free at that time and safe for the predicted path of the objects, it transitions to DECIDING. If it is safe for a certain period of time, it moves to DECIDED. + +![path_deciding](./images/goal_planner-deciding_path.drawio.svg) + +## Unimplemented parts / limitations + +- Only shift pull over can be executed concurrently with other modules +- Parking in tight spots and securing margins are traded off. A mode is needed to reduce the margin by using a slower speed depending on the situation, but there is no mechanism for dynamic switching of speeds. +- Parking space available depends on visibility of objects, and sometimes parking decisions cannot be made properly. +- Margin to unrecognized objects(Not even unknown objects) depends on the occupancy grid. May get too close to unrecognized ground objects because the objects that are allowed to approach (e.g., grass, leaves) are indistinguishable. + +Unimplemented parts / limitations for freespace parking + +- When a short path is generated, the ego does can not drive with it. +- Complex cases take longer to generate or fail. +- The drivable area is not guaranteed to fit in the parking_lot. diff --git a/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml similarity index 100% rename from planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml rename to planning/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml diff --git a/planning/behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/arc_backward_parking.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/arc_forward_parking.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/goal_planner-deciding_path.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_candidates.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/goal_planner-goal_distance.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/goal_planner-is_safe.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/goal_planner-longitudinal_margin.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/goal_planner-object_to_avoid.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/goal_planner-refined_goal.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/goal_planner-safety_check.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/goal_planner-threads.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png b/planning/autoware_behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png rename to planning/autoware_behavior_path_goal_planner_module/images/goal_priority_object_to_avoid_rviz.png diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png b/planning/autoware_behavior_path_goal_planner_module/images/goal_priority_rviz.png similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_priority_rviz.png rename to planning/autoware_behavior_path_goal_planner_module/images/goal_priority_rviz.png diff --git a/planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png b/planning/autoware_behavior_path_goal_planner_module/images/goal_priority_with_goal.png similarity index 100% rename from planning/behavior_path_goal_planner_module/images/goal_priority_with_goal.png rename to planning/autoware_behavior_path_goal_planner_module/images/goal_priority_with_goal.png diff --git a/planning/behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/path_goal_refinement.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/pull_over_freespace_parking_flowchart.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/images/shift_parking.drawio.svg b/planning/autoware_behavior_path_goal_planner_module/images/shift_parking.drawio.svg similarity index 100% rename from planning/behavior_path_goal_planner_module/images/shift_parking.drawio.svg rename to planning/autoware_behavior_path_goal_planner_module/images/shift_parking.drawio.svg diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/default_fixed_goal_planner.hpp similarity index 75% rename from planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp rename to planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/default_fixed_goal_planner.hpp index 58203573d0c35..44604084ef480 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/default_fixed_goal_planner.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ -#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ -#include "behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" +#include "autoware_behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { class DefaultFixedGoalPlanner : public FixedGoalPlannerBase @@ -40,6 +40,6 @@ class DefaultFixedGoalPlanner : public FixedGoalPlannerBase const PathWithLaneId & refined_path, const std::shared_ptr & planner_data) const; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DEFAULT_FIXED_GOAL_PLANNER_HPP_ diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/fixed_goal_planner_base.hpp similarity index 77% rename from planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp rename to planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index 9112500e10ac3..2a14bfaf72118 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" #include #include @@ -26,7 +26,7 @@ using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; using tier4_planning_msgs::msg::PathWithLaneId; -namespace behavior_path_planner +namespace autoware::behavior_path_planner { class FixedGoalPlannerBase @@ -48,6 +48,6 @@ class FixedGoalPlannerBase protected: BehaviorModuleOutput previous_module_output_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/freespace_pull_over.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/freespace_pull_over.hpp new file mode 100644 index 0000000000000..f5e5df05474cd --- /dev/null +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/freespace_pull_over.hpp @@ -0,0 +1,54 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ + +#include "autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp" + +#include +#include +#include + +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::RRTStar; + +class FreespacePullOver : public PullOverPlannerBase +{ +public: + FreespacePullOver( + rclcpp::Node & node, const GoalPlannerParameters & parameters, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + + PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } + + std::optional plan(const Pose & goal_pose) override; + +protected: + std::unique_ptr planner_; + double velocity_; + bool use_back_; + bool left_side_parking_; +}; +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FREESPACE_PULL_OVER_HPP_ diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/geometric_pull_over.hpp similarity index 76% rename from planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp rename to planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/geometric_pull_over.hpp index a76ee5f7529f0..3d72c3089bd77 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ -#include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include +#include #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; class GeometricPullOver : public PullOverPlannerBase { public: @@ -63,6 +63,6 @@ class GeometricPullOver : public PullOverPlannerBase GeometricParallelParking planner_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_module.hpp similarity index 90% rename from planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp rename to planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_module.hpp index 012af88f4ca70..4ecb11ec25776 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_module.hpp @@ -12,25 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ -#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ - -#include "behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "behavior_path_goal_planner_module/freespace_pull_over.hpp" -#include "behavior_path_goal_planner_module/geometric_pull_over.hpp" -#include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "behavior_path_goal_planner_module/goal_searcher.hpp" -#include "behavior_path_goal_planner_module/shift_pull_over.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" - -#include -#include -#include +#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ + +#include "autoware_behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware_behavior_path_goal_planner_module/freespace_pull_over.hpp" +#include "autoware_behavior_path_goal_planner_module/geometric_pull_over.hpp" +#include "autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware_behavior_path_goal_planner_module/goal_searcher.hpp" +#include "autoware_behavior_path_goal_planner_module/shift_pull_over.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include #include #include @@ -48,27 +48,27 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using autoware::lane_departure_checker::LaneDepartureChecker; using autoware_vehicle_msgs::msg::HazardLightsCommand; using geometry_msgs::msg::PoseArray; -using lane_departure_checker::LaneDepartureChecker; using nav_msgs::msg::OccupancyGrid; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -using freespace_planning_algorithms::AbstractPlanningAlgorithm; -using freespace_planning_algorithms::AstarParam; -using freespace_planning_algorithms::AstarSearch; -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 autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; +using autoware::freespace_planning_algorithms::AstarParam; +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::PlannerCommonParam; +using autoware::freespace_planning_algorithms::RRTStar; +using autoware::freespace_planning_algorithms::RRTStarParam; + +using autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using tier4_autoware_utils::Polygon2d; #define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ @@ -465,7 +465,7 @@ class GoalPlannerModule : public SceneModuleInterface mutable std::shared_ptr objects_filtering_params_; mutable std::shared_ptr safety_check_params_; - vehicle_info_util::VehicleInfo vehicle_info_{}; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; // planner std::vector> pull_over_planners_; @@ -666,6 +666,6 @@ class GoalPlannerModule : public SceneModuleInterface void setDebugData(); void printParkingPositionError() const; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp similarity index 80% rename from planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp rename to planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp index f6c39ea2d6176..7ecd7063d623e 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -13,25 +13,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include -#include +#include +#include +#include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { -using freespace_planning_algorithms::AstarParam; -using freespace_planning_algorithms::PlannerCommonParam; -using freespace_planning_algorithms::RRTStarParam; +using autoware::freespace_planning_algorithms::AstarParam; +using autoware::freespace_planning_algorithms::PlannerCommonParam; +using autoware::freespace_planning_algorithms::RRTStarParam; enum class ParkingPolicy { LEFT_SIDE = 0, @@ -126,6 +126,6 @@ struct GoalPlannerParameters // debug bool print_debug_info{false}; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher.hpp similarity index 85% rename from planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp rename to planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher.hpp index 89f4086874183..5897207a3b357 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ -#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ -#include "behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware_behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using tier4_autoware_utils::LinearRing2d; using BasicPolygons2d = std::vector; @@ -70,6 +70,6 @@ class GoalSearcher : public GoalSearcherBase LinearRing2d vehicle_footprint_{}; bool left_side_parking_{true}; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher_base.hpp similarity index 80% rename from planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp rename to planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher_base.hpp index f5d879358cd38..984374ae166c5 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ -#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ -#include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using geometry_msgs::msg::Pose; using tier4_autoware_utils::MultiPolygon2d; @@ -76,6 +76,6 @@ class GoalSearcherBase Pose reference_goal_pose_{}; MultiPolygon2d area_polygons_{}; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/manager.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/manager.hpp new file mode 100644 index 0000000000000..291fbab7cafc3 --- /dev/null +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/manager.hpp @@ -0,0 +1,58 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ + +#include "autoware_behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ + +class GoalPlannerModuleManager : public SceneModuleManagerInterface +{ +public: + GoalPlannerModuleManager() : SceneModuleManagerInterface{"goal_planner"} {} + + void init(rclcpp::Node * node) override; + + std::unique_ptr createNewSceneModuleInstance() override + { + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); + } + + void updateModuleParams(const std::vector & parameters) override; + + bool isAlwaysExecutableModule() const override; + + bool isSimultaneousExecutableAsApprovedModule() const override; + + bool isSimultaneousExecutableAsCandidateModule() const override; + +private: + std::shared_ptr parameters_; +}; + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp similarity index 85% rename from planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp rename to planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp index 8da00f872f443..7a27b2f43b4b3 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ -#include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_goal_planner_module/goal_planner_parameters.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" #include #include @@ -29,7 +29,7 @@ using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; using tier4_planning_msgs::msg::PathWithLaneId; -namespace behavior_path_planner +namespace autoware::behavior_path_planner { enum class PullOverPlannerType { NONE = 0, @@ -112,7 +112,7 @@ class PullOverPlannerBase public: PullOverPlannerBase(rclcpp::Node & node, const GoalPlannerParameters & parameters) { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } @@ -133,12 +133,12 @@ class PullOverPlannerBase protected: std::shared_ptr planner_data_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; GoalPlannerParameters parameters_; BehaviorModuleOutput previous_module_output_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/shift_pull_over.hpp similarity index 79% rename from planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp rename to planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/shift_pull_over.hpp index 635816f17497f..2296875e1d8d6 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/shift_pull_over.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ -#define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ -#include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" +#include "autoware_behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include +#include #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; class ShiftPullOver : public PullOverPlannerBase { @@ -57,6 +57,6 @@ class ShiftPullOver : public PullOverPlannerBase bool left_side_parking_{true}; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__SHIFT_PULL_OVER_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/util.hpp b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/util.hpp new file mode 100644 index 0000000000000..78e23b979094c --- /dev/null +++ b/planning/autoware_behavior_path_goal_planner_module/include/autoware_behavior_path_goal_planner_module/util.hpp @@ -0,0 +1,113 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ + +#include "autoware_behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" + +#include + +#include "visualization_msgs/msg/detail/marker_array__struct.hpp" +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner::goal_planner_utils +{ +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::PathWithLaneId; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +using Polygon2d = tier4_autoware_utils::Polygon2d; + +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance); + +/* + * @brief expand pull_over_lanes to the opposite side of drivable roads by bound_offset. + * bound_offset must be positive regardless of left_side is true/false + */ +lanelet::ConstLanelets generateExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, const double bound_offset); + +lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( + const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, + const geometry_msgs::msg::Pose ego_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double outer_road_offset, + const double inner_road_offset); +PredictedObjects extractObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects); +PredictedObjects filterObjectsByLateralDistance( + const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, + const double distance_thresh, const bool filter_inside); +PredictedObjects extractStaticObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects, + const double velocity_thresh); + +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path); +bool isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_thresh); + +std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose); +PathWithLaneId cropForwardPoints( + const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const double extend_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const Pose & extend_pose); + +std::vector createPathFootPrints( + const PathWithLaneId & path, const double base_to_front, const double base_to_rear, + const double width); + +// debug +MarkerArray createPullOverAreaMarkerArray( + const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const std_msgs::msg::ColorRGBA & color, const double z); +MarkerArray createPosesMarkerArray( + const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); +MarkerArray createTextsMarkerArray( + const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); +MarkerArray createGoalCandidatesMarkerArray( + const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); +MarkerArray createLaneletPolygonMarkerArray( + const lanelet::CompoundPolygon3d & polygon, const std_msgs::msg::Header & header, + const std::string & ns, const std_msgs::msg::ColorRGBA & color); +MarkerArray createNumObjectsToAvoidTextsMarkerArray( + const GoalCandidates & goal_candidates, std::string && ns, + const std_msgs::msg::ColorRGBA & color); +} // namespace autoware::behavior_path_planner::goal_planner_utils + +#endif // AUTOWARE_BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/autoware_behavior_path_goal_planner_module/package.xml b/planning/autoware_behavior_path_goal_planner_module/package.xml new file mode 100644 index 0000000000000..952173564888a --- /dev/null +++ b/planning/autoware_behavior_path_goal_planner_module/package.xml @@ -0,0 +1,40 @@ + + + + autoware_behavior_path_goal_planner_module + 0.1.0 + The autoware_behavior_path_goal_planner_module package + + Kosuke Takeuchi + Kyoichi Sugahara + Satoshi OTA + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + Mamoru Sobue + Daniel Sanchez + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_path_planner + autoware_behavior_path_planner_common + autoware_rtc_interface + motion_utils + pluginlib + rclcpp + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_path_goal_planner_module/plugins.xml b/planning/autoware_behavior_path_goal_planner_module/plugins.xml new file mode 100644 index 0000000000000..5c9489ed102ba --- /dev/null +++ b/planning/autoware_behavior_path_goal_planner_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp similarity index 91% rename from planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp rename to planning/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index 750efe89de6ab..48fc8d519161c 100644 --- a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware_behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_goal_planner_module/util.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using Point2d = tier4_autoware_utils::Point2d; using tier4_planning_msgs::msg::PathWithLaneId; @@ -105,4 +105,4 @@ PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( return refined_path; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp similarity index 87% rename from planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp rename to planning/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp index 6d3eb09bf4352..15f17668e5e2b 100644 --- a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp +++ b/planning/autoware_behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -12,27 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_goal_planner_module/freespace_pull_over.hpp" +#include "autoware_behavior_path_goal_planner_module/freespace_pull_over.hpp" -#include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_goal_planner_module/util.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) : PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { - freespace_planning_algorithms::VehicleShape vehicle_shape( + autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); if (parameters.freespace_parking_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; @@ -137,4 +137,4 @@ std::optional FreespacePullOver::plan(const Pose & goal_pose) return pull_over_path; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/planning/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp similarity index 89% rename from planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp rename to planning/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp index 0779002690f8f..1ff47142dadb1 100644 --- a/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp +++ b/planning/autoware_behavior_path_goal_planner_module/src/geometric_pull_over.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_goal_planner_module/geometric_pull_over.hpp" +#include "autoware_behavior_path_goal_planner_module/geometric_pull_over.hpp" -#include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_goal_planner_module/util.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include #include @@ -23,7 +23,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { GeometricPullOver::GeometricPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, @@ -79,4 +79,4 @@ std::optional GeometricPullOver::plan(const Pose & goal_pose) return pull_over_path; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp similarity index 98% rename from planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp rename to planning/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 4acc4dce743ad..9532def19029d 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_goal_planner_module/goal_planner_module.hpp" - -#include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_goal_planner_module/goal_planner_module.hpp" + +#include "autoware_behavior_path_goal_planner_module/util.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -39,7 +39,7 @@ #include #include -using behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; +using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -50,7 +50,7 @@ using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::inverseTransformPose; -namespace behavior_path_planner +namespace autoware::behavior_path_planner { GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, @@ -60,7 +60,7 @@ GoalPlannerModule::GoalPlannerModule( objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, thread_safe_data_{mutex_, clock_}, is_lane_parking_cb_running_{false}, is_freespace_parking_cb_running_{false}, @@ -95,7 +95,7 @@ GoalPlannerModule::GoalPlannerModule( // set selected goal searcher // currently there is only one goal_searcher_type - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info.createFootprint(); goal_searcher_ = std::make_shared(*parameters, vehicle_footprint_); @@ -2152,8 +2152,8 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData( static std::vector filterObjectsByWithinPolicy( const std::shared_ptr & objects, const lanelet::ConstLanelets & target_lanes, - const std::shared_ptr & - params) + const std::shared_ptr< + autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams> & params) { // implanted part of behavior_path_planner::utils::path_safety_checker::filterObjects() and // createTargetObjectsOnLane() @@ -2229,7 +2229,7 @@ std::pair GoalPlannerModule::isSafePath( 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( + autoware::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); @@ -2289,7 +2289,7 @@ std::pair GoalPlannerModule::isSafePath( CollisionCheckDebugMap collision_check{}; const bool current_is_safe = std::invoke([&]() { if (parameters.safety_check_params.method == "RSS") { - return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( + return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_over_path, ego_predicted_path, filtered_objects, collision_check, planner_data->parameters, safety_check_params->rss_params, objects_filtering_params->use_all_predicted_path, hysteresis_factor); @@ -2627,4 +2627,4 @@ void GoalPlannerModule::GoalPlannerData::update( goal_searcher->setReferenceGoal(goal_searcher_->getReferenceGoal()); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp similarity index 97% rename from planning/behavior_path_goal_planner_module/src/goal_searcher.cpp rename to planning/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index df91687622349..c268431b63836 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_goal_planner_module/goal_searcher.hpp" +#include "autoware_behavior_path_goal_planner_module/goal_searcher.hpp" -#include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_goal_planner_module/util.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/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" @@ -30,9 +30,9 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; @@ -545,4 +545,4 @@ GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( return *closest_goal_candidate; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/autoware_behavior_path_goal_planner_module/src/manager.cpp new file mode 100644 index 0000000000000..be5845d2dd37d --- /dev/null +++ b/planning/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -0,0 +1,886 @@ +// 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 "autoware_behavior_path_goal_planner_module/manager.hpp" + +#include "autoware_behavior_path_goal_planner_module/util.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +void GoalPlannerModuleManager::init(rclcpp::Node * node) +{ + // init manager interface + initInterface(node, {""}); + + GoalPlannerParameters p; + + const std::string base_ns = "goal_planner."; + // general params + { + 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 + { + 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 = + node->declare_parameter(ns + "backward_goal_search_length"); + p.goal_search_interval = node->declare_parameter(ns + "goal_search_interval"); + p.longitudinal_margin = node->declare_parameter(ns + "longitudinal_margin"); + p.max_lateral_offset = node->declare_parameter(ns + "max_lateral_offset"); + p.lateral_offset_interval = node->declare_parameter(ns + "lateral_offset_interval"); + p.ignore_distance_from_lane_start = + node->declare_parameter(ns + "ignore_distance_from_lane_start"); + p.margin_from_boundary = node->declare_parameter(ns + "margin_from_boundary"); + + const std::string parking_policy_name = + node->declare_parameter(ns + "parking_policy"); + if (parking_policy_name == "left_side") { + p.parking_policy = ParkingPolicy::LEFT_SIDE; + } else if (parking_policy_name == "right_side") { + p.parking_policy = ParkingPolicy::RIGHT_SIDE; + } else { + RCLCPP_ERROR_STREAM( + node->get_logger().get_child(name()), + "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); + exit(EXIT_FAILURE); + } + } + + // occupancy grid map + { + 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"); + p.obstacle_threshold = node->declare_parameter(ns + "obstacle_threshold"); + } + + // 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_soft_margins = + node->declare_parameter>(ns + "collision_check_soft_margins"); + p.object_recognition_collision_check_hard_margins = + node->declare_parameter>(ns + "collision_check_hard_margins"); + 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"); + p.detection_bound_offset = node->declare_parameter(ns + "detection_bound_offset"); + p.outer_road_detection_offset = + node->declare_parameter(ns + "outer_road_detection_offset"); + p.inner_road_detection_offset = + node->declare_parameter(ns + "inner_road_detection_offset"); + + // validate object recognition collision check margins + if ( + p.object_recognition_collision_check_soft_margins.empty() || + p.object_recognition_collision_check_hard_margins.empty()) { + RCLCPP_FATAL_STREAM( + node->get_logger().get_child(name()), + "object_recognition.collision_check_soft_margins and " + << "object_recognition.collision_check_hard_margins must not be empty. " + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + } + + // pull over general params + { + 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 + { + 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"); + p.minimum_lateral_jerk = node->declare_parameter(ns + "minimum_lateral_jerk"); + p.deceleration_interval = node->declare_parameter(ns + "deceleration_interval"); + p.after_shift_straight_distance = + 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 + { + 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"); + p.parallel_parking_parameters.forward_parking_velocity = + node->declare_parameter(ns + "forward_parking_velocity"); + p.parallel_parking_parameters.forward_parking_lane_departure_margin = + node->declare_parameter(ns + "forward_parking_lane_departure_margin"); + p.parallel_parking_parameters.forward_parking_path_interval = + node->declare_parameter(ns + "forward_parking_path_interval"); + p.parallel_parking_parameters.forward_parking_max_steer_angle = + node->declare_parameter(ns + "forward_parking_max_steer_angle"); // 20deg + } + + // forward 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 = + node->declare_parameter(ns + "after_backward_parking_straight_distance"); + p.parallel_parking_parameters.backward_parking_velocity = + node->declare_parameter(ns + "backward_parking_velocity"); + p.parallel_parking_parameters.backward_parking_lane_departure_margin = + node->declare_parameter(ns + "backward_parking_lane_departure_margin"); + p.parallel_parking_parameters.backward_parking_path_interval = + node->declare_parameter(ns + "backward_parking_path_interval"); + p.parallel_parking_parameters.backward_parking_max_steer_angle = + node->declare_parameter(ns + "backward_parking_max_steer_angle"); // 20deg + } + + // freespace parking general params + { + 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"); + p.freespace_parking_velocity = node->declare_parameter(ns + "velocity"); + p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); + p.freespace_parking_common_parameters.time_limit = + node->declare_parameter(ns + "time_limit"); + p.freespace_parking_common_parameters.minimum_turning_radius = + node->declare_parameter(ns + "minimum_turning_radius"); + p.freespace_parking_common_parameters.maximum_turning_radius = + node->declare_parameter(ns + "maximum_turning_radius"); + p.freespace_parking_common_parameters.turning_radius_size = + node->declare_parameter(ns + "turning_radius_size"); + p.freespace_parking_common_parameters.maximum_turning_radius = std::max( + p.freespace_parking_common_parameters.maximum_turning_radius, + p.freespace_parking_common_parameters.minimum_turning_radius); + p.freespace_parking_common_parameters.turning_radius_size = + std::max(p.freespace_parking_common_parameters.turning_radius_size, 1); + } + + // freespace parking search config + { + 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 = + node->declare_parameter(ns + "angle_goal_range"); + p.freespace_parking_common_parameters.curve_weight = + node->declare_parameter(ns + "curve_weight"); + p.freespace_parking_common_parameters.reverse_weight = + node->declare_parameter(ns + "reverse_weight"); + p.freespace_parking_common_parameters.lateral_goal_range = + node->declare_parameter(ns + "lateral_goal_range"); + p.freespace_parking_common_parameters.longitudinal_goal_range = + node->declare_parameter(ns + "longitudinal_goal_range"); + } + + // 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 + { + 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"); + p.astar_parameters.distance_heuristic_weight = + node->declare_parameter(ns + "distance_heuristic_weight"); + } + + // 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"); + p.rrt_star_parameters.max_planning_time = + node->declare_parameter(ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); + 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"); + } + + const std::string path_safety_check_ns = "goal_planner.path_safety_check."; + + // EgoPredictedPath + const 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 + "min_acceleration"); + 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 + const 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 + const 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 + const 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 + const 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.keep_unsafe_time = + node->declare_parameter(safety_check_ns + "keep_unsafe_time"); + p.safety_check_params.method = node->declare_parameter(safety_check_ns + "method"); + 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 + const 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"); + } + + // IntegralPredictedPolygonParams + const std::string integral_ns = safety_check_ns + "integral_predicted_polygon_params."; + { + p.safety_check_params.integral_predicted_polygon_params.forward_margin = + node->declare_parameter(integral_ns + "forward_margin"); + p.safety_check_params.integral_predicted_polygon_params.backward_margin = + node->declare_parameter(integral_ns + "backward_margin"); + p.safety_check_params.integral_predicted_polygon_params.lat_margin = + node->declare_parameter(integral_ns + "lat_margin"); + p.safety_check_params.integral_predicted_polygon_params.time_horizon = + node->declare_parameter(integral_ns + "time_horizon"); + } + + // debug + { + const std::string ns = base_ns + "debug."; + p.print_debug_info = node->declare_parameter(ns + "print_debug_info"); + } + + // validation of parameters + if (p.shift_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + node->get_logger().get_child(name()), + "shift_sampling_num must be positive integer. Given parameter: " + << p.shift_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + if (p.maximum_deceleration < 0.0) { + RCLCPP_FATAL_STREAM( + node->get_logger().get_child(name()), + "maximum_deceleration cannot be negative value. Given parameter: " + << p.maximum_deceleration << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + + parameters_ = std::make_shared(p); +} + +void GoalPlannerModuleManager::updateModuleParams( + [[maybe_unused]] const std::vector & parameters) +{ + // TODO(someone): This function does not check OR update + // object_recognition_collision_check_soft_margins, + // object_recognition_collision_check_hard_margins, maximum_deceleration, shift_sampling_num or + // parking_policy, there seems to be a problem when we use a temp value to check these values. + + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + const std::string base_ns = "goal_planner."; + // general params + + { + updateParam(parameters, base_ns + "th_stopped_velocity", p->th_stopped_velocity); + updateParam(parameters, base_ns + "th_arrived_distance", p->th_arrived_distance); + updateParam(parameters, base_ns + "th_stopped_time", p->th_stopped_time); + + updateParam( + parameters, base_ns + "center_line_path_interval", p->center_line_path_interval); + } + // goal search + + { + const std::string ns = base_ns + "goal_search."; + updateParam(parameters, ns + "goal_priority", p->goal_priority); + updateParam( + parameters, ns + "minimum_weighted_distance.lateral_weight", + p->minimum_weighted_distance_lateral_weight); + updateParam( + parameters, ns + "prioritize_goals_before_objects", p->prioritize_goals_before_objects); + updateParam( + parameters, ns + "forward_goal_search_length", p->forward_goal_search_length); + updateParam( + parameters, ns + "backward_goal_search_length", p->backward_goal_search_length); + updateParam(parameters, ns + "goal_search_interval", p->goal_search_interval); + updateParam(parameters, ns + "longitudinal_margin", p->longitudinal_margin); + updateParam(parameters, ns + "max_lateral_offset", p->max_lateral_offset); + updateParam(parameters, ns + "lateral_offset_interval", p->lateral_offset_interval); + updateParam( + parameters, ns + "ignore_distance_from_lane_start", p->ignore_distance_from_lane_start); + updateParam(parameters, ns + "margin_from_boundary", p->margin_from_boundary); + } + + // occupancy grid map + { + const std::string ns = base_ns + "occupancy_grid."; + updateParam( + parameters, ns + "use_occupancy_grid_for_goal_search", p->use_occupancy_grid_for_goal_search); + updateParam( + parameters, ns + "use_occupancy_grid_for_path_collision_check", + p->use_occupancy_grid_for_path_collision_check); + updateParam( + parameters, ns + "use_occupancy_grid_for_goal_longitudinal_margin", + p->use_occupancy_grid_for_goal_longitudinal_margin); + updateParam( + parameters, ns + "occupancy_grid_collision_check_margin", + p->occupancy_grid_collision_check_margin); + updateParam(parameters, ns + "theta_size", p->theta_size); + updateParam(parameters, ns + "obstacle_threshold", p->obstacle_threshold); + } + + // object recognition + { + const std::string ns = base_ns + "object_recognition."; + + updateParam(parameters, ns + "use_object_recognition", p->use_object_recognition); + updateParam( + parameters, ns + "object_recognition_collision_check_max_extra_stopping_margin", + p->object_recognition_collision_check_max_extra_stopping_margin); + updateParam(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity); + updateParam(parameters, ns + "detection_bound_offset", p->detection_bound_offset); + updateParam(parameters, ns + "outer_road_detection_offset", p->outer_road_detection_offset); + updateParam(parameters, ns + "inner_road_detection_offset", p->inner_road_detection_offset); + } + + // pull over general params + { + const std::string ns = base_ns + "pull_over."; + + updateParam( + parameters, ns + "minimum_request_length", p->pull_over_minimum_request_length); + updateParam(parameters, ns + "pull_over_velocity", p->pull_over_velocity); + updateParam( + parameters, ns + "pull_over_minimum_velocity", p->pull_over_minimum_velocity); + updateParam(parameters, ns + "decide_path_distance", p->decide_path_distance); + updateParam(parameters, ns + "maximum_jerk", p->maximum_jerk); + updateParam(parameters, ns + "path_priority", p->path_priority); + updateParam>( + parameters, ns + "efficient_path_order", p->efficient_path_order); + } + + // shift parking + { + const std::string ns = base_ns + "pull_over.shift_parking."; + updateParam(parameters, ns + "enable_shift_parking", p->enable_shift_parking); + updateParam(parameters, ns + "maximum_lateral_jerk", p->maximum_lateral_jerk); + updateParam(parameters, ns + "minimum_lateral_jerk", p->minimum_lateral_jerk); + updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); + updateParam( + parameters, ns + "after_shift_straight_distance", p->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 + { + const std::string ns = base_ns + "pull_over.parallel_parking.forward."; + updateParam(parameters, ns + "enable_arc_forward_parking", p->enable_arc_forward_parking); + updateParam( + parameters, ns + "after_forward_parking_straight_distance", + p->parallel_parking_parameters.after_forward_parking_straight_distance); + updateParam( + parameters, ns + "forward_parking_velocity", + p->parallel_parking_parameters.forward_parking_velocity); + updateParam( + parameters, ns + "forward_parking_lane_departure_margin", + p->parallel_parking_parameters.forward_parking_lane_departure_margin); + updateParam( + parameters, ns + "forward_parking_path_interval", + p->parallel_parking_parameters.forward_parking_path_interval); + updateParam( + parameters, ns + "forward_parking_max_steer_angle", + p->parallel_parking_parameters.forward_parking_max_steer_angle); + } + + // forward parallel parking backward + { + const std::string ns = base_ns + "pull_over.parallel_parking.backward."; + updateParam( + parameters, ns + "enable_arc_backward_parking", p->enable_arc_backward_parking); + updateParam( + parameters, ns + "after_backward_parking_straight_distance", + p->parallel_parking_parameters.after_backward_parking_straight_distance); + updateParam( + parameters, ns + "backward_parking_velocity", + p->parallel_parking_parameters.backward_parking_velocity); + updateParam( + parameters, ns + "backward_parking_lane_departure_margin", + p->parallel_parking_parameters.backward_parking_lane_departure_margin); + updateParam( + parameters, ns + "backward_parking_path_interval", + p->parallel_parking_parameters.backward_parking_path_interval); + updateParam( + parameters, ns + "backward_parking_max_steer_angle", + p->parallel_parking_parameters.backward_parking_max_steer_angle); + } + + // freespace parking general params + { + const std::string ns = base_ns + "pull_over.freespace_parking."; + updateParam(parameters, ns + "enable_freespace_parking", p->enable_freespace_parking); + updateParam( + parameters, ns + "freespace_parking_algorithm", p->freespace_parking_algorithm); + updateParam(parameters, ns + "velocity", p->freespace_parking_velocity); + + updateParam(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); + updateParam( + parameters, ns + "time_limit", p->freespace_parking_common_parameters.time_limit); + updateParam( + parameters, ns + "minimum_turning_radius", + p->freespace_parking_common_parameters.minimum_turning_radius); + updateParam( + parameters, ns + "maximum_turning_radius", + p->freespace_parking_common_parameters.maximum_turning_radius); + updateParam( + parameters, ns + "turning_radius_size", + p->freespace_parking_common_parameters.turning_radius_size); + p->freespace_parking_common_parameters.maximum_turning_radius = std::max( + p->freespace_parking_common_parameters.maximum_turning_radius, + p->freespace_parking_common_parameters.minimum_turning_radius); + p->freespace_parking_common_parameters.turning_radius_size = + std::max(p->freespace_parking_common_parameters.turning_radius_size, 1); + } + + // freespace parking search config + { + const std::string ns = base_ns + "pull_over.freespace_parking.search_configs."; + updateParam( + parameters, ns + "theta_size", p->freespace_parking_common_parameters.theta_size); + updateParam( + parameters, ns + "angle_goal_range", p->freespace_parking_common_parameters.angle_goal_range); + updateParam( + parameters, ns + "curve_weight", p->freespace_parking_common_parameters.curve_weight); + updateParam( + parameters, ns + "reverse_weight", p->freespace_parking_common_parameters.reverse_weight); + updateParam( + parameters, ns + "lateral_goal_range", + p->freespace_parking_common_parameters.lateral_goal_range); + updateParam( + parameters, ns + "longitudinal_goal_range", + p->freespace_parking_common_parameters.longitudinal_goal_range); + } + + // freespace parking costmap configs + { + const std::string ns = base_ns + "pull_over.freespace_parking.costmap_configs."; + updateParam( + parameters, ns + "obstacle_threshold", + p->freespace_parking_common_parameters.obstacle_threshold); + } + + // freespace parking astar + { + const std::string ns = base_ns + "pull_over.freespace_parking.astar."; + updateParam( + parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions); + updateParam(parameters, ns + "use_back", p->astar_parameters.use_back); + updateParam( + parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight); + } + + // freespace parking rrtstar + + { + const std::string ns = base_ns + "pull_over.freespace_parking.rrtstar."; + updateParam(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); + updateParam( + parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling); + updateParam( + parameters, ns + "max_planning_time", p->rrt_star_parameters.max_planning_time); + updateParam(parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius); + updateParam(parameters, ns + "margin", p->rrt_star_parameters.margin); + } + + // stop condition + { + updateParam( + parameters, base_ns + "stop_condition.maximum_deceleration_for_stop", + p->maximum_deceleration_for_stop); + updateParam( + parameters, base_ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); + } + + const std::string path_safety_check_ns = "goal_planner.path_safety_check."; + const std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; + + // EgoPredictedPath + { + updateParam( + parameters, ego_path_ns + "min_velocity", p->ego_predicted_path_params.min_velocity); + updateParam( + parameters, ego_path_ns + "min_acceleration", p->ego_predicted_path_params.acceleration); + updateParam( + parameters, ego_path_ns + "time_horizon_for_front_object", + p->ego_predicted_path_params.time_horizon_for_front_object); + updateParam( + parameters, ego_path_ns + "time_horizon_for_rear_object", + p->ego_predicted_path_params.time_horizon_for_rear_object); + updateParam( + parameters, ego_path_ns + "time_resolution", p->ego_predicted_path_params.time_resolution); + updateParam( + parameters, ego_path_ns + "delay_until_departure", + p->ego_predicted_path_params.delay_until_departure); + } + + // ObjectFilteringParams + const std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; + { + updateParam( + parameters, obj_filter_ns + "safety_check_time_horizon", + p->objects_filtering_params.safety_check_time_horizon); + updateParam( + parameters, obj_filter_ns + "safety_check_time_resolution", + p->objects_filtering_params.safety_check_time_resolution); + updateParam( + parameters, obj_filter_ns + "object_check_forward_distance", + p->objects_filtering_params.object_check_forward_distance); + updateParam( + parameters, obj_filter_ns + "object_check_backward_distance", + p->objects_filtering_params.object_check_backward_distance); + updateParam( + parameters, obj_filter_ns + "ignore_object_velocity_threshold", + p->objects_filtering_params.ignore_object_velocity_threshold); + updateParam( + parameters, obj_filter_ns + "include_opposite_lane", + p->objects_filtering_params.include_opposite_lane); + updateParam( + parameters, obj_filter_ns + "invert_opposite_lane", + p->objects_filtering_params.invert_opposite_lane); + updateParam( + parameters, obj_filter_ns + "check_all_predicted_path", + p->objects_filtering_params.check_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_all_predicted_path", + p->objects_filtering_params.use_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_predicted_path_outside_lanelet", + p->objects_filtering_params.use_predicted_path_outside_lanelet); + } + + // ObjectTypesToCheck + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + updateParam( + parameters, obj_types_ns + "check_car", + p->objects_filtering_params.object_types_to_check.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->objects_filtering_params.object_types_to_check.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->objects_filtering_params.object_types_to_check.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->objects_filtering_params.object_types_to_check.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->objects_filtering_params.object_types_to_check.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->objects_filtering_params.object_types_to_check.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->objects_filtering_params.object_types_to_check.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->objects_filtering_params.object_types_to_check.check_pedestrian); + } + // ObjectLaneConfiguration + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + updateParam( + parameters, obj_lane_ns + "check_current_lane", + p->objects_filtering_params.object_lane_configuration.check_current_lane); + updateParam( + parameters, obj_lane_ns + "check_right_side_lane", + p->objects_filtering_params.object_lane_configuration.check_right_lane); + updateParam( + parameters, obj_lane_ns + "check_left_side_lane", + p->objects_filtering_params.object_lane_configuration.check_left_lane); + updateParam( + parameters, obj_lane_ns + "check_shoulder_lane", + p->objects_filtering_params.object_lane_configuration.check_shoulder_lane); + updateParam( + parameters, obj_lane_ns + "check_other_lane", + p->objects_filtering_params.object_lane_configuration.check_other_lane); + } + + // SafetyCheckParams + const std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; + { + updateParam( + parameters, safety_check_ns + "enable_safety_check", + p->safety_check_params.enable_safety_check); + updateParam( + parameters, safety_check_ns + "keep_unsafe_time", p->safety_check_params.keep_unsafe_time); + updateParam(parameters, safety_check_ns + "method", p->safety_check_params.method); + updateParam( + parameters, safety_check_ns + "hysteresis_factor_expand_rate", + p->safety_check_params.hysteresis_factor_expand_rate); + updateParam( + parameters, safety_check_ns + "backward_path_length", + p->safety_check_params.backward_path_length); + updateParam( + parameters, safety_check_ns + "forward_path_length", + p->safety_check_params.forward_path_length); + updateParam( + parameters, safety_check_ns + "publish_debug_marker", + p->safety_check_params.publish_debug_marker); + } + + // RSSparams + const std::string rss_ns = safety_check_ns + "rss_params."; + { + updateParam( + parameters, rss_ns + "rear_vehicle_reaction_time", + p->safety_check_params.rss_params.rear_vehicle_reaction_time); + updateParam( + parameters, rss_ns + "rear_vehicle_safety_time_margin", + p->safety_check_params.rss_params.rear_vehicle_safety_time_margin); + updateParam( + parameters, rss_ns + "lateral_distance_max_threshold", + p->safety_check_params.rss_params.lateral_distance_max_threshold); + updateParam( + parameters, rss_ns + "longitudinal_distance_min_threshold", + p->safety_check_params.rss_params.longitudinal_distance_min_threshold); + updateParam( + parameters, rss_ns + "longitudinal_velocity_delta_time", + p->safety_check_params.rss_params.longitudinal_velocity_delta_time); + } + + // IntegralPredictedPolygonParams + const std::string integral_ns = safety_check_ns + "integral_predicted_polygon_params."; + { + updateParam( + parameters, integral_ns + "forward_margin", + p->safety_check_params.integral_predicted_polygon_params.forward_margin); + updateParam( + parameters, integral_ns + "backward_margin", + p->safety_check_params.integral_predicted_polygon_params.backward_margin); + updateParam( + parameters, integral_ns + "lat_margin", + p->safety_check_params.integral_predicted_polygon_params.lat_margin); + updateParam( + parameters, integral_ns + "time_horizon", + p->safety_check_params.integral_predicted_polygon_params.time_horizon); + } + + // debug + { + const std::string ns = base_ns + "debug."; + updateParam(parameters, ns + "print_debug_info", p->print_debug_info); + } + + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); + }); +} + +bool GoalPlannerModuleManager::isAlwaysExecutableModule() const +{ + // enable AlwaysExecutable whenever goal modification is not allowed + // because only minor path refinements are made for fixed goals + if (!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 (!utils::isAllowedGoalModification(planner_data_->route_handler)) { + return true; + } + + return config_.enable_simultaneous_execution_as_approved_module; +} + +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 (!utils::isAllowedGoalModification(planner_data_->route_handler)) { + return true; + } + + return config_.enable_simultaneous_execution_as_candidate_module; +} + +} // namespace autoware::behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_path_planner::GoalPlannerModuleManager, + autoware::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp similarity index 97% rename from planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp rename to planning/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp index 376adb178708a..5c1e874b949ab 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/autoware_behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_goal_planner_module/shift_pull_over.hpp" +#include "autoware_behavior_path_goal_planner_module/shift_pull_over.hpp" -#include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_goal_planner_module/util.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include #include @@ -24,7 +24,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { ShiftPullOver::ShiftPullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, @@ -324,4 +324,4 @@ double ShiftPullOver::calcBeforeShiftedArcLength( return before_arc_length; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/autoware_behavior_path_goal_planner_module/src/util.cpp new file mode 100644 index 0000000000000..aa3b61899978c --- /dev/null +++ b/planning/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -0,0 +1,450 @@ +// Copyright 2021 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 "autoware_behavior_path_goal_planner_module/util.hpp" + +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner::goal_planner_utils +{ + +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; + +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.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; + + const auto target_shoulder_lane = route_handler.getPullOverTarget(goal_pose); + if (target_shoulder_lane) { + // pull over on shoulder lane + return route_handler.getShoulderLaneletSequence( + *target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance); + } + + 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, false, true); + } else { + outermost_lane = route_handler.getMostRightLanelet(closest_lane, false, true); + } + + constexpr bool only_route_lanes = false; + return route_handler.getLaneletSequence( + outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); +} + +lanelet::ConstLanelets generateExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, const double bound_offset) +{ + const auto pull_over_lanes = + getPullOverLanes(route_handler, left_side, backward_distance, forward_distance); + + return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0) + : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, -bound_offset); +} + +static double getOffsetToLanesBoundary( + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose target_pose, + const bool left_side) +{ + lanelet::ConstLanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(lanelet_sequence, target_pose, &closest_lanelet); + + // the boundary closer to ego. if left_side, take right boundary + const auto & boundary3d = left_side ? closest_lanelet.rightBound() : closest_lanelet.leftBound(); + const auto boundary = lanelet::utils::to2D(boundary3d); + using lanelet::utils::conversion::toLaneletPoint; + const auto arc_coords = lanelet::geometry::toArcCoordinates( + boundary, lanelet::utils::to2D(toLaneletPoint(target_pose.position)).basicPoint()); + return arc_coords.distance; +} + +lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( + const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, + const geometry_msgs::msg::Pose ego_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double outer_road_offset, + const double inner_road_offset) +{ + const double front_overhang = vehicle_info.front_overhang_m, + wheel_base = vehicle_info.wheel_base_m, wheel_tread = vehicle_info.wheel_tread_m; + const double side_overhang = + left_side ? vehicle_info.left_overhang_m : vehicle_info.right_overhang_m; + const double ego_length_to_front = wheel_base + front_overhang; + const double ego_width_to_front = + !left_side ? (-wheel_tread / 2.0 - side_overhang) : (wheel_tread / 2.0 + side_overhang); + tier4_autoware_utils::Point2d front_edge_local{ego_length_to_front, ego_width_to_front}; + const auto front_edge_glob = tier4_autoware_utils::transformPoint( + front_edge_local, tier4_autoware_utils::pose2transform(ego_pose)); + geometry_msgs::msg::Pose ego_front_pose; + ego_front_pose.position = + createPoint(front_edge_glob.x(), front_edge_glob.y(), ego_pose.position.z); + + // ========================================================================================== + // NOTE: the point which is on the right side of a directed line has negative distance + // getExpandedLanelet(1.0, -2.0) expands a lanelet by 1.0 to the left and by 2.0 to the right + // ========================================================================================== + const double ego_offset_to_closer_boundary = + getOffsetToLanesBoundary(pull_over_lanes, ego_front_pose, left_side); + return left_side ? lanelet::utils::getExpandedLanelets( + pull_over_lanes, outer_road_offset, + ego_offset_to_closer_boundary - inner_road_offset) + : lanelet::utils::getExpandedLanelets( + pull_over_lanes, ego_offset_to_closer_boundary + inner_road_offset, + -outer_road_offset); +} + +PredictedObjects extractObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects) +{ + const auto lanes = generateExpandedPullOverLanes( + route_handler, left_side, backward_distance, forward_distance, bound_offset); + + const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( + objects, lanes, [](const auto & obj, const auto & lanelet) { + return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet); + }); + + return objects_in_lanes; +} + +PredictedObjects extractStaticObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects, + const double velocity_thresh) +{ + const auto objects_in_lanes = extractObjectsInExpandedPullOverLanes( + route_handler, left_side, backward_distance, forward_distance, bound_offset, objects); + + return utils::path_safety_checker::filterObjectsByVelocity(objects_in_lanes, velocity_thresh); +} + +PredictedObjects filterObjectsByLateralDistance( + const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, + const double distance_thresh, const bool filter_inside) +{ + PredictedObjects filtered_objects; + for (const auto & object : objects.objects) { + const double distance = + utils::calcLateralDistanceFromEgoToObject(ego_pose, vehicle_width, object); + if (filter_inside ? distance < distance_thresh : distance > distance_thresh) { + filtered_objects.objects.push_back(object); + } + } + + return filtered_objects; +} + +MarkerArray createPullOverAreaMarkerArray( + const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const std_msgs::msg::ColorRGBA & color, const double z) +{ + MarkerArray marker_array{}; + for (size_t i = 0; i < area_polygons.size(); ++i) { + Marker marker = createDefaultMarker( + header.frame_id, header.stamp, "pull_over_area_" + std::to_string(i), i, + visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), color); + const auto & poly = area_polygons.at(i); + for (const auto & p : poly.outer()) { + marker.points.push_back(createPoint(p.x(), p.y(), z)); + } + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + +MarkerArray createPosesMarkerArray( + const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color) +{ + MarkerArray msg{}; + int32_t i = 0; + for (const auto & pose : poses) { + Marker marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::ARROW, + createMarkerScale(0.5, 0.25, 0.25), color); + marker.pose = pose; + marker.id = i++; + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createGoalPriorityTextsMarkerArray( + const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color) +{ + MarkerArray msg{}; + int32_t i = 0; + for (const auto & pose : poses) { + 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, 0, 1.0); + marker.id = i; + marker.text = std::to_string(i); + msg.markers.push_back(marker); + i++; + } + + return msg; +} + +MarkerArray createNumObjectsToAvoidTextsMarkerArray( + const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color) +{ + MarkerArray msg{}; + int32_t i = 0; + for (const auto & goal_candidate : goal_candidates) { + 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 : + 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; +} + +MarkerArray createLaneletPolygonMarkerArray( + const lanelet::CompoundPolygon3d & polygon, const std_msgs::msg::Header & header, + const std::string & ns, const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array{}; + auto marker = createDefaultMarker( + header.frame_id, header.stamp, ns, 0, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), color); + for (const auto & p : polygon) { + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + marker_array.markers.push_back(marker); + return marker_array; +} + +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path) +{ + double lateral_deviation = 0.0; + for (const auto & target_point : target_path.points) { + const size_t nearest_index = + motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position); + lateral_deviation = std::max( + lateral_deviation, + std::abs(tier4_autoware_utils::calcLateralDeviation( + reference_path.points[nearest_index].point.pose, target_point.point.pose.position))); + } + return lateral_deviation; +} + +bool isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_threshold) +{ + return calcLateralDeviationBetweenPaths(reference_path, target_path) < + lateral_deviation_threshold; +} + +std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose) +{ + const size_t end_idx = motion_utils::findNearestSegmentIndex(path.points, end_pose.position); + std::vector clipped_points{ + path.points.begin(), path.points.begin() + end_idx}; + if (clipped_points.empty()) { + return std::nullopt; + } + + // add projected end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); + projected_point.point.pose = + tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_path = path; + clipped_path.points = clipped_points; + + return clipped_path; +} + +PathWithLaneId cropForwardPoints( + const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length) +{ + const auto & points = path.points; + + double sum_length = 0; + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + const double seg_length = tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (forward_length < sum_length + seg_length) { + const auto cropped_points = + std::vector{points.begin() + target_seg_idx, points.begin() + i}; + PathWithLaneId cropped_path = path; + cropped_path.points = cropped_points; + + // add precise end pose to cropped points + const double remaining_length = forward_length - sum_length; + const Pose precise_end_pose = + calcOffsetPose(cropped_path.points.back().point.pose, remaining_length, 0, 0); + if (remaining_length < 0.1) { + // if precise_end_pose is too close, replace the last point + cropped_path.points.back().point.pose = precise_end_pose; + } else { + auto precise_end_point = cropped_path.points.back(); + precise_end_point.point.pose = precise_end_pose; + cropped_path.points.push_back(precise_end_point); + } + return cropped_path; + } + sum_length += seg_length; + } + + // if forward_length is too long, return points after target_seg_idx + const auto cropped_points = + std::vector{points.begin() + target_seg_idx, points.end()}; + PathWithLaneId cropped_path = path; + cropped_path.points = cropped_points; + return cropped_path; +} + +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const double extend_length) +{ + const auto & target_terminal_pose = target_path.points.back().point.pose; + + // generate clipped road lane reference path from previous module path terminal pose to shift end + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); + + PathWithLaneId clipped_path = + cropForwardPoints(reference_path, target_path_terminal_idx, extend_length); + + // shift clipped path to previous module path terminal pose + const double lateral_shift_from_reference_path = + motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); + for (auto & p : clipped_path.points) { + p.point.pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, lateral_shift_from_reference_path, 0); + } + + auto extended_path = target_path; + const auto start_point = + std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { + const bool is_forward = + tier4_autoware_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose).x > + 0.0; + const bool is_close = tier4_autoware_utils::calcDistance2d( + p.point.pose.position, target_terminal_pose.position) < 0.1; + return is_forward && !is_close; + }); + std::copy(start_point, clipped_path.points.end(), std::back_inserter(extended_path.points)); + + extended_path.points = motion_utils::removeOverlapPoints(extended_path.points); + + return extended_path; +} + +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const Pose & extend_pose) +{ + const auto & target_terminal_pose = target_path.points.back().point.pose; + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); + const double extend_distance = motion_utils::calcSignedArcLength( + reference_path.points, target_path_terminal_idx, extend_pose.position); + + return extendPath(target_path, reference_path, extend_distance); +} + +std::vector createPathFootPrints( + const PathWithLaneId & path, const double base_to_front, const double base_to_rear, + const double width) +{ + std::vector footprints; + for (const auto & point : path.points) { + const auto & pose = point.point.pose; + footprints.push_back( + tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width)); + } + return footprints; +} + +} // namespace autoware::behavior_path_planner::goal_planner_utils diff --git a/planning/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/autoware_behavior_path_lane_change_module/CMakeLists.txt new file mode 100644 index 0000000000000..b0d9967e65ddd --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_path_lane_change_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/interface.cpp + src/manager.cpp + src/scene.cpp + src/utils/markers.cpp + src/utils/utils.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + test/test_lane_change_utils.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/autoware_behavior_path_lane_change_module/README.md b/planning/autoware_behavior_path_lane_change_module/README.md new file mode 100644 index 0000000000000..a0870fe428c8a --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/README.md @@ -0,0 +1,820 @@ +# Lane Change design + +The Lane Change module is activated when lane change is needed and can be safely executed. + +## Lane Change Requirement + +- As the prerequisite, the type of lane boundary in the HD map has to be one of the following: + - Dashed lane marking: Lane changes are permitted in both directions. + - Dashed marking on the left and solid on the right: Lane changes are allowed from left to right. + - Dashed marking on the right and solid on the left: Lane changes are allowed from right to left. + - `allow_lane_change` tags is set as `true` +- During lane change request condition + - The ego-vehicle isn’t on a `preferred_lane`. + - There is neither intersection nor crosswalk on the path of the lane change +- lane change ready condition + - Path of the lane change does not collide with other dynamic objects (see the figure below) + - Lane change candidate path is approved by an operator. + +## Generating Lane Change Candidate Path + +The lane change candidate path is divided into two phases: preparation and lane-changing. The following figure illustrates each phase of the lane change candidate path. + +![lane-change-phases](./images/lane_change-lane_change_phases.png) + +### Preparation phase + +The preparation trajectory is the candidate path's first and the straight portion generated along the ego vehicle's current lane. The length of the preparation trajectory is computed as follows. + +```C++ +lane_change_prepare_distance = current_speed * lane_change_prepare_duration + 0.5 * deceleration * lane_change_prepare_duration^2 +``` + +During the preparation phase, the turn signal will be activated when the remaining distance is equal to or less than `lane_change_search_distance`. + +### Lane-changing phase + +The lane-changing phase consist of the shifted path that moves ego from current lane to the target lane. Total distance of lane-changing phase is as follows. Note that during the lane changing phase, the ego vehicle travels at a constant speed. + +```C++ +lane_change_prepare_velocity = std::max(current_speed + deceleration * lane_change_prepare_duration, minimum_lane_changing_velocity) +lane_changing_distance = lane_change_prepare_velocity * lane_changing_duration +``` + +The `backward_length_buffer_for_end_of_lane` is added to allow some window for any possible delay, such as control or mechanical delay during brake lag. + +#### Multiple candidate path samples (longitudinal acceleration) + +Lane change velocity is affected by the ego vehicle's current velocity. High velocity requires longer preparation and lane changing distance. However we also need to plan lane changing trajectories in case ego vehicle slows down. +Computing candidate paths that assumes ego vehicle's slows down is performed by substituting predetermined deceleration value into `prepare_length`, `prepare_velocity` and `lane_changing_length` equation. + +The predetermined longitudinal acceleration values are a set of value that starts from `longitudinal_acceleration = maximum_longitudinal_acceleration`, and decrease by `longitudinal_acceleration_resolution` until it reaches `longitudinal_acceleration = -maximum_longitudinal_deceleration`. Both `maximum_longitudinal_acceleration` and `maximum_longitudinal_deceleration` are calculated as: defined in the `common.param` file as `normal.min_acc`. + +```C++ +maximum_longitudinal_acceleration = min(common_param.max_acc, lane_change_param.max_acc) +maximum_longitudinal_deceleration = max(common_param.min_acc, lane_change_param.min_acc) +``` + +where `common_param` is vehicle common parameter, which defines vehicle common maximum longitudinal acceleration and deceleration. Whereas, `lane_change_param` has maximum longitudinal acceleration and deceleration for the lane change module. For example, if a user set and `common_param.max_acc=1.0` and `lane_change_param.max_acc=0.0`, `maximum_longitudinal_acceleration` becomes `0.0`, and the lane change does not accelerate in the lane change phase. + +The `longitudinal_acceleration_resolution` is determine by the following + +```C++ +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 chart illustrates the conditions under which longitudinal acceleration values are sampled. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +if (prev_module_path is empty?) then (yes) + :Return empty list; + stop +else (no) +endif + +if (max_acc <= 0.0) then (yes) + :Return **sampled acceleration values**; + note left: Calculated sampled acceleration using\ngetAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num) + stop +endif + +if (max_lane_change_length > ego's distance to the end of the current lanes.) then (yes) + :Return **sampled acceleration values**; + stop +endif + +if (isVehicleStuck(current_lanes)) then (yes) + :Return **sampled acceleration values**; + stop +else (no) +endif + +if (is goal is in target lanes) then (yes) + if (max_lane_change_length < ego's distance to the goal along the target lanes) then (yes) + :Return {max_acc}; + stop + else (no) + endif +else (no) + if (max_lane_change_length < ego's distance to the end of the target lanes.) then (yes) + :Return {max_acc}; + stop + else (no) + endif +endif + +:Return **sampled acceleration values**; +stop + +@enduml + +``` + +while the following describes the process by which longitudinal accelerations are sampled. + +```plantuml +@startuml +start +:Initialize sampled_values with min_acc; + +if (min_acc > max_acc) then (yes) + :Return empty list; + stop +elseif (max_acc - min_acc < epsilon) then (yes) + :Return {0.0}; + stop +else (no) + :Calculate resolution; +endif + +:Start loop from min_acc to max_acc with resolution step; +repeat + if (sampled_values.back() < -epsilon AND next_value > epsilon) then (yes) + :Insert 0.0 into sampled_values; + endif + :Add sampled_acc to sampled_values; + repeat while (sampled_acc < max_acc + epsilon) is (TRUE) + +:Return sampled_values; +stop +@enduml +``` + +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](./images/lane_change-candidate_path_samples.png) + +Which path will be chosen will depend on validity and collision check. + +#### Multiple candidate path samples (lateral acceleration) + +In addition to sampling longitudinal acceleration, we also sample lane change paths by adjusting the value of lateral acceleration. Since lateral acceleration influences the duration of a lane change, a lower lateral acceleration value results in a longer lane change path, while a higher lateral acceleration value leads to a shorter lane change path. This allows the lane change module to generate a shorter lane change path by increasing the lateral acceleration when there is limited space for the lane change. + +The maximum and minimum lateral accelerations are defined in the lane change parameter file as a map. The range of lateral acceleration is determined for each velocity by linearly interpolating the values in the map. Let's assume we have the following map + +| Ego Velocity | Minimum lateral acceleration | Maximum lateral acceleration | +| :----------- | ---------------------------- | ---------------------------- | +| 0.0 | 0.2 | 0.3 | +| 2.0 | 0.2 | 0.4 | +| 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.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_acceleration) / lateral_acceleration_sampling_num +``` + +#### Candidate Path's validity check + +A candidate path is considered valid if it meets the following criteria: + +1. The distance from the ego vehicle's current position to the end of the current lanes is sufficient to perform a single lane change. +2. The distance from the ego vehicle's current position to the goal along the current lanes is adequate to complete multiple lane changes. +3. The distance from the ego vehicle's current position to the end of the target lanes is adequate for completing multiple lane changes. +4. Intersection requirements are met (conditions are parameterized). +5. Crosswalk requirements are satisfied (conditions are parameterized). +6. Traffic light regulations are adhered to (conditions are parameterized). +7. The lane change can be completed after passing a parked vehicle. +8. The lane change is deemed safe to execute. + +The following flow chart illustrates the validity check. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #White + +start +if (Check if start point is valid by check if it is covered by neighbour lanes polygon) then (not covered) + #LightPink:Reject path; + stop +else (covered) +endif + +group check for distance #LightYellow + :Calculate total length and goal related distances; + if (total lane change length considering single lane change > distance from current pose to end of current lanes) then (yes) + #LightPink:Reject path; + stop + else (no) + endif + + if (goal is in current lanes) then (yes) + if (total lane change length considering multiple lane changes > distance from ego to goal along current lanes) then (yes) + #LightPink:Reject path; + stop + else (no) + endif + else (no) + endif + + if (target lanes is empty) then (yes) + #LightPink:Reject path; + stop + else (no) + endif + if (total lane change length considering multiple lane changes > distance from ego to the end of target lanes) then (yes) + #LightPink:Reject path; + stop + else (no) + endif +end group + + + +group evaluate on Crosswalk #LightCyan +if (regulate_on_crosswalk and not enough length to crosswalk) then (yes) + if (stop time < stop time threshold\n(Related to stuck detection)) then (yes) + #LightPink:Reject path; + stop + else (no) + :Allow lane change in crosswalk; + endif +else (no) +endif +end group + +group evaluate on Intersection #LightGreen +if (regulate_on_intersection and not enough length to intersection) then (yes) + if (stop time < stop time threshold\n(Related to stuck detection)) then (yes) + #LightPink:Reject path; + stop + else (no) + :Allow lane change in intersection; + endif +else (no) +endif +end group + +group evaluate on Traffic Light #Lavender +if (regulate_on_traffic_light and not enough length to complete lane change before stop line) then (yes) + #LightPink:Reject path; + stop +elseif (stopped at red traffic light within distance) then (yes) + #LightPink:Reject path; + stop +else (no) +endif +end group + +if (ego is not stuck but parked vehicle exists in target lane) then (yes) + #LightPink:Reject path; + stop +else (no) +endif + +if (is safe to perform lane change) then (yes) + #Cyan:Return candidate path list; + stop +else (no) + #LightPink:Reject path; +endif + +stop + +@enduml +``` + +#### Candidate Path's Safety check + +See [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) + +#### Objects selection and classification + +First, we divide the target objects into obstacles in the target lane, obstacles in the current lane, and obstacles in other lanes. Target lane indicates the lane that the ego vehicle is going to reach after the lane change and current lane mean the current lane where the ego vehicle is following before the lane change. Other lanes are lanes that do not belong to the target and current lanes. The following picture describes objects on each lane. Note that users can remove objects either on current and other lanes from safety check by changing the flag, which are `check_objects_on_current_lanes` and `check_objects_on_other_lanes`. + +![object lanes](./images/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. The explanation for parked car detection is written in [documentation for avoidance module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md). + +The detection area for the target lane can be expanded beyond its original boundaries to enable detection of objects that are outside the target lane's limits. + +
+ + + + + +
+
+
Without Lane Expansion
+ Without lane expansion +
+ 		+
+
With Lane Expansion
+ With lane expansion +
+
+
+ +##### Object filtering + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title NormalLaneChange::filterObjects Method Execution Flow + +start + +group "Filter Objects by Class" { +:Iterate through each object in objects list; +while (has not finished iterating through object list) is (TRUE) + if (current object type != param.object_types_to_check?) then (TRUE) + #LightPink:Remove current object; +else (FALSE) + :Keep current object; +endif +end while +end group + +if (object list is empty?) then (TRUE) + :Return empty result; + stop +else (FALSE) +endif + +group "Filter Oncoming Objects" #PowderBlue { +:Iterate through each object in target lane objects list; +while (has not finished iterating through object list?) is (TRUE) +:check object's yaw with reference to ego's yaw.; +if (yaw difference < 90 degree?) then (TRUE) + :Keep current object; +else (FALSE) +if (object is stopping?) then (TRUE) + :Keep current object; +else (FALSE) + #LightPink:Remove current object; +endif +endif +endwhile +end group + +if (object list is empty?) then (TRUE) + :Return empty result; + stop +else (FALSE) +endif + +group "Filter Objects Ahead Terminal" #Beige { +:Calculate lateral distance from ego to current lanes center; + +:Iterate through each object in objects list; +while (has not finished iterating through object list) is (TRUE) + :Get current object's polygon; + :Initialize distance to terminal from object to max; + while (has not finished iterating through object polygon's vertices) is (TRUE) + :Calculate object's lateral distance to end of lane; + :Update minimum distance to terminal from object; + end while + if (Is object's distance to terminal exceeds minimum lane change length?) then (TRUE) + #LightPink:Remove current object; + else (FALSE) + endif +end while +end group + +if (object list is empty?) then (TRUE) + :Return empty result; + stop +else (FALSE) +endif + +group "Filter Objects By Lanelets" #LightGreen { + +:Iterate through each object in objects list; +while (has not finished iterating through object list) is (TRUE) + :lateral distance diff = difference between object's lateral distance and ego's lateral distance to the current lanes' centerline.; + if (Object in target lane polygon, and lateral distance diff is more than half of ego's width?) then (TRUE) + :Add to target_lane_objects; + else (FALSE) + if (Object overlaps with backward target lanes?) then (TRUE) + :Add to target_lane_objects; + else (FALSE) + if (Object in current lane polygon?) then (TRUE) + :Add to current_lane_objects; + else (FALSE) + :Add to other_lane_objects; + endif + endif + endif +end while + +:Return target lanes object, current lanes object and other lanes object; +end group + +:Generate path from current lanes; + +if (path empty?) then (TRUE) + :Return empty result; + stop +else (FALSE) +endif + +group "Filter Target Lanes' objects" #LightCyan { + +:Iterate through each object in target lane objects list; +while (has not finished iterating through object list) is (TRUE) + :check object's velocity; + if(velocity is within threshold?) then (TRUE) + :Keep current object; + else (FALSE) + :check whether object is ahead of ego; + if(object is ahead of ego?) then (TRUE) + :keep current object; + else (FALSE) + #LightPink:remove current object; + endif + endif +endwhile +end group + +group "Filter Current Lanes' objects" #LightYellow { + +:Iterate through each object in current lane objects list; +while (has not finished iterating through object list) is (TRUE) + :check object's velocity; + if(velocity is within threshold?) then (TRUE) + :check whether object is ahead of ego; + if(object is ahead of ego?) then (TRUE) + :keep current object; + else (FALSE) + #LightPink:remove current object; + endif + else (FALSE) + #LightPink:remove current object; + endif +endwhile +end group + +group "Filter Other Lanes' objects" #Lavender { + +:Iterate through each object in other lane objects list; +while (has not finished iterating through object list) is (TRUE) + :check object's velocity; + if(velocity is within threshold?) then (TRUE) + :check whether object is ahead of ego; + if(object is ahead of ego?) then (TRUE) + :keep current object; + else (FALSE) + #LightPink:remove current object; + endif + else (FALSE) + #LightPink:remove current object; + endif +endwhile +end group + +:Trasform the objects into extended predicted object and return them as lane_change_target_objects; +stop + +@enduml +``` + +##### Collision check in prepare phase + +The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases. + +![enable collision check at prepare phase](./images/lane_change-enable_collision_check_at_prepare_phase.png) + +The parameter `prepare_phase_ignore_target_speed_thresh` can be configured to ignore the prepare phase collision check for targets whose speeds are less than a specific threshold, such as stationary or very slow-moving objects. + +#### If the lane is blocked and multiple lane changes + +When driving on the public road with other vehicles, there exist scenarios where lane changes cannot be executed. Suppose the candidate path is evaluated as unsafe, for example, due to incoming vehicles in the adjacent lane. In that case, the ego vehicle can't change lanes, and it is impossible to reach the goal. Therefore, the ego vehicle must stop earlier at a certain distance and wait for the adjacent lane to be evaluated as safe. The minimum stopping distance can be computed from shift length and minimum lane changing velocity. + +```C++ +lane_changing_time = f(shift_length, lat_acceleration, lat_jerk) +minimum_lane_change_distance = minimum_prepare_length + minimum_lane_changing_velocity * lane_changing_time + lane_change_finish_judge_buffer +``` + +The following figure illustrates when the lane is blocked in multiple lane changes cases. + +![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) + +#### Stopping position when an object exists ahead + +When an obstacle is in front of the ego vehicle, stop with keeping a distance for lane change. +The position to be stopped depends on the situation, such as when the lane change is blocked by the target lane obstacle, or when the lane change is not needed immediately.The following shows the division in that case. + +##### When the ego vehicle is near the end of the lane change + +Regardless of the presence or absence of objects in the lane change target lane, stop by keeping the distance necessary for lane change to the object ahead. + +![stop_at_terminal_no_block](./images/lane_change-stop_at_terminal_no_block.drawio.svg) + +![stop_at_terminal](./images/lane_change-stop_at_terminal.drawio.svg) + +##### When the ego vehicle is not near the end of the lane change + +If there are NO objects in the lane change section of the target lane, stop by keeping the distance necessary for lane change to the object ahead. + +![stop_not_at_terminal_no_blocking_object](./images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg) + +If there are objects in the lane change section of the target lane, stop WITHOUT keeping the distance necessary for lane change to the object ahead. + +![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) + +##### When the target lane is far away + +When the target lane for lane change is far away and not next to the current lane, do not keep the distance necessary for lane change to the object ahead. + +![stop_far_from_target_lane](./images/lane_change-stop_far_from_target_lane.drawio.svg) + +### Lane Change When Stuck + +The ego vehicle is considered stuck if it is stopped and meets any of the following conditions: + +- There is an obstacle in front of the current lane +- The ego vehicle is at the end of the current lane + +In this case, the safety check for lane change is relaxed compared to normal times. +Please refer to the 'stuck' section under the 'Collision checks during lane change' for more details. +The function to stop by keeping a margin against forward obstacle in the previous section is being performed to achieve this feature. + +### 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. + +The following depicts the flow of the abort lane change check. + +```plantuml +@startuml +skinparam monochrome true +skinparam defaultTextAlignment center + +title Abort Lane Change + +while(Lane Following) + if (Lane Change required) then (**YES**) + if (Safe to change lane) then (**SAFE**) + while(Lane Changing) + if (Lane Change Completed) then (**YES**) + break + else (**NO**) + if (Is Abort Condition Satisfied) then (**NO**) + else (**YES**) + if (Is Enough margin to retry lane change) then (**YES**) + if (Ego is on lane change prepare phase) then (**YES**) + :Cancel lane change; + break + else (**NO**) + if (Will the overhang to target lane be less than threshold?) then (**YES**) + :Perform abort maneuver; + break + else (NO) + :Stop or Cruise depending on the situation; + endif + endif + else (**NO**) + endif + endif + endif + :Stop and wait; + endwhile + else (**UNSAFE**) + endif + else (**NO**) + endif +endwhile +-[hidden]-> +detach +@enduml +``` + +To preventive measure for lane change path oscillations caused by alternating safe and unsafe conditions, an additional hysteresis count check is implemented before executing an abort or cancel maneuver. If unsafe, the `unsafe_hysteresis_count_` is incremented and compared against `unsafe_hysteresis_threshold`; exceeding it prompts an abort condition check, ensuring decisions are made with consideration to recent safety assessments as shown in flow chart above. This mechanism stabilizes decision-making, preventing abrupt changes due to transient unsafe conditions. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Abort Lane Change + +if (Perform collision check?) then (SAFE) + :Reset unsafe_hysteresis_count_; +else (UNSAFE) + :Increase unsafe_hysteresis_count_; + if (unsafe_hysteresis_count_ > unsafe_hysteresis_threshold?) then (FALSE) + else (TRUE) + #LightPink:Check abort condition; + stop + endif +endif +:Continue lane changing; +@enduml +``` + +#### Cancel + +Suppose the lane change trajectory is evaluated as unsafe. In that case, if the ego vehicle has not departed from the current lane yet, the trajectory will be reset, and the ego vehicle will resume the lane following the maneuver. + +The function can be enabled by setting `enable_on_prepare_phase` to `true`. + +The following image illustrates the cancel process. + +![cancel](./images/lane_change-cancel.png) + +#### Abort + +Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN'T GUARANTEE a safe abort process, as it didn't check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both `enable_on_prepare_phase` and `enable_on_lane_changing_phase` to `true`. The parameter `max_lateral_jerk` need to be set to a high value in order for it to work. + +![abort](./images/lane_change-abort.png) + +#### Stop/Cruise + +The last behavior will also occur if the ego vehicle has departed from the current lane. If the abort function is disabled or the abort is no longer possible, the ego vehicle will attempt to stop or transition to the obstacle cruise mode. Do note that the module DOESN'T GUARANTEE safe maneuver due to the unexpected behavior that might've occurred during these critical scenarios. The following images illustrate the situation. + +![stop](./images/lane_change-cant_cancel_no_abort.png) + +## Parameters + +### Essential lane change parameters + +The following parameters are configurable in [lane_change.param.yaml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml) + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | +| `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.0 | +| `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | +| `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | +| `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | +| `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 2.0 | +| `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | +| `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | +| `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | +| `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 | 3 | +| `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 | 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 | +| `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] | +| `lateral_acceleration.min_values` | [m/ss] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | +| `lateral_acceleration.max_values` | [m/ss] | double | Max lateral acceleration values corresponding to velocity (look up table) | [0.65, 0.65, 0.65] | + +### Lane change regulations + +| Name | Unit | Type | Description | Default value | +| :------------------------- | ---- | ------- | ---------------------------------------------------------- | ------------- | +| `regulation.crosswalk` | [-] | boolean | Allow lane change in between crosswalks | true | +| `regulation.intersection` | [-] | boolean | Allow lane change in between intersections | true | +| `regulation.traffic_light` | [-] | boolean | Allow lane change to be performed in between traffic light | true | + +### 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 + +#### Target Objects + +| Name | Unit | Type | Description | Default value | +| :------------------------- | ---- | ------- | ------------------------------------------- | ------------- | +| `target_object.car` | [-] | boolean | Include car objects for safety check | true | +| `target_object.truck` | [-] | boolean | Include truck objects for safety check | true | +| `target_object.bus` | [-] | boolean | Include bus objects for safety check | true | +| `target_object.trailer` | [-] | boolean | Include trailer objects for safety check | true | +| `target_object.unknown` | [-] | boolean | Include unknown objects for safety check | true | +| `target_object.bicycle` | [-] | boolean | Include bicycle objects for safety check | true | +| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | +| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | + +#### common + +| Name | Unit | Type | Description | Default value | +| :----------------------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.lane_expansion.left_offset` | [m] | double | Expand the left boundary of the detection area, allowing objects previously outside on the left to be detected and registered as targets. | 0.0 | +| `safety_check.lane_expansion.right_offset` | [m] | double | Expand the right boundary of the detection area, allowing objects previously outside on the right to be detected and registered as targets. | 0.0 | + +#### Additional parameters + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `enable_collision_check_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | +| `enable_collision_check_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | true | +| `enable_collision_check_for_prepare_phase.turns` | [-] | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase. | true | +| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | +| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | +| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | + +#### safety constraints during lane change path is computed + +| Name | Unit | Type | Description | Default value | +| :----------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.execution.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.execution.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.execution.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.execution.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 1.0 | +| `safety_check.execution.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `safety_check.execution.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | + +##### safety constraints to cancel lane change path + +| Name | Unit | Type | Description | Default value | +| :-------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.cancel.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.cancel.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -2.0 | +| `safety_check.cancel.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 1.5 | +| `safety_check.cancel.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 0.8 | +| `safety_check.cancel.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 1.0 | +| `safety_check.cancel.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 2.5 | +| `safety_check.cancel.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.6 | + +##### safety constraints used during lane change path is computed when ego is stuck + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------- | ------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `safety_check.stuck.expected_front_deceleration` | [m/s^2] | double | The front object's maximum deceleration when the front vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.stuck.expected_rear_deceleration` | [m/s^2] | double | The rear object's maximum deceleration when the rear vehicle perform sudden braking. (\*1) | -1.0 | +| `safety_check.stuck.rear_vehicle_reaction_time` | [s] | double | The reaction time of the rear vehicle driver which starts from the driver noticing the sudden braking of the front vehicle until the driver step on the brake. | 2.0 | +| `safety_check.stuck.rear_vehicle_safety_time_margin` | [s] | double | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking. | 1.0 | +| `safety_check.stuck.lateral_distance_max_threshold` | [m] | double | The lateral distance threshold that is used to determine whether lateral distance between two object is enough and whether lane change is safe. | 2.0 | +| `safety_check.stuck.longitudinal_distance_min_threshold` | [m] | double | The longitudinal distance threshold that is used to determine whether longitudinal distance between two object is enough and whether lane change is safe. | 3.0 | +| `safety_check.stuck.longitudinal_velocity_delta_time` | [m] | double | The time multiplier that is used to compute the actual gap between vehicle at each predicted points (not RSS distance) | 0.8 | + +(\*1) the value must be negative. + +### Abort lane change + +The following parameters are configurable in `lane_change.param.yaml`. + +| Name | Unit | Type | Description | Default value | +| :------------------------------------- | ------- | ------- | ---------------------------------------------------------------------------------------------------------------- | ------------- | +| `cancel.enable_on_prepare_phase` | [-] | boolean | Enable cancel lane change | true | +| `cancel.enable_on_lane_changing_phase` | [-] | boolean | Enable abort lane change. | false | +| `cancel.delta_time` | [s] | double | The time taken to start steering to return to the center line. | 3.0 | +| `cancel.duration` | [s] | double | The time taken to complete returning to the center line. | 3.0 | +| `cancel.max_lateral_jerk` | [m/sss] | double | The maximum lateral jerk for abort path | 1000.0 | +| `cancel.overhang_tolerance` | [m] | double | Lane change cancel is prohibited if the vehicle head exceeds the lane boundary more than this tolerance distance | 0.0 | +| `unsafe_hysteresis_threshold` | [-] | int | threshold that helps prevent frequent switching between safe and unsafe decisions | 10 | + +### Debug + +The following parameters are configurable in `lane_change.param.yaml`. + +| Name | Unit | Type | Description | Default value | +| :--------------------- | ---- | ------- | ---------------------------- | ------------- | +| `publish_debug_marker` | [-] | boolean | Flag to publish debug marker | false | + +## Debug Marker & Visualization + +To enable the debug marker, execute (no restart is needed) + +```shell +ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner lane_change.publish_debug_marker true + +``` + +or simply set the `publish_debug_marker` to `true` in the `lane_change.param.yaml` for permanent effect (restart is needed). + +Then add the marker + +```shell +/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left +``` + +in `rviz2`. + +![debug](./images/lane_change-debug-1.png) + +![debug2](./images/lane_change-debug-2.png) + +![debug3](./images/lane_change-debug-3.png) + +Available information + +1. Ego to object relation, plus safety check information +2. Ego vehicle interpolated pose up to the latest safety check position. +3. Object is safe or not, shown by the color of the polygon (Green = Safe, Red = unsafe) +4. Valid candidate paths. +5. Position when lane changing start and end. diff --git a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml similarity index 100% rename from planning/behavior_path_lane_change_module/config/lane_change.param.yaml rename to planning/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml diff --git a/planning/behavior_path_lane_change_module/images/lane_change-abort.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-abort.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-abort.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-abort.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-cancel.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-cancel.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-debug-1.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-1.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-debug-1.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-1.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-debug-2.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-2.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-debug-2.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-2.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-debug-3.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-3.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-debug-3.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-debug-3.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-enable_collision_check_at_prepare_phase.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-intersection_case.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-intersection_case.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-intersection_case.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-intersection_case.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_change_phases.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-lane_change_phases.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_change_phases.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-with.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-lane_expansion-without.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg b/planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal.drawio.svg diff --git a/planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg b/planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_at_terminal_no_block.drawio.svg diff --git a/planning/behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg b/planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_far_from_target_lane.drawio.svg diff --git a/planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg b/planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal.drawio.svg diff --git a/planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg b/planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-stop_not_at_terminal_no_blocking_object.drawio.svg diff --git a/planning/behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png b/planning/autoware_behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png rename to planning/autoware_behavior_path_lane_change_module/images/lane_change-when_cannot_change_lanes.png diff --git a/planning/behavior_path_lane_change_module/images/lane_change.drawio.svg b/planning/autoware_behavior_path_lane_change_module/images/lane_change.drawio.svg similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_change.drawio.svg rename to planning/autoware_behavior_path_lane_change_module/images/lane_change.drawio.svg diff --git a/planning/behavior_path_lane_change_module/images/lane_objects.drawio.svg b/planning/autoware_behavior_path_lane_change_module/images/lane_objects.drawio.svg similarity index 100% rename from planning/behavior_path_lane_change_module/images/lane_objects.drawio.svg rename to planning/autoware_behavior_path_lane_change_module/images/lane_objects.drawio.svg diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp new file mode 100644 index 0000000000000..5ad4660dcf39c --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/interface.hpp @@ -0,0 +1,159 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ + +#include "autoware_behavior_path_lane_change_module/scene.hpp" +#include "autoware_behavior_path_lane_change_module/utils/base_class.hpp" +#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware_behavior_path_lane_change_module/utils/path.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + +#include + +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::objects_of_interest_marker_interface::ColorName; +using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::PathWithLaneId; + +class LaneChangeInterface : public SceneModuleInterface +{ +public: + LaneChangeInterface( + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map, + std::unique_ptr && module_type); + + LaneChangeInterface(const LaneChangeInterface &) = delete; + LaneChangeInterface(LaneChangeInterface &&) = delete; + LaneChangeInterface & operator=(const LaneChangeInterface &) = delete; + LaneChangeInterface & operator=(LaneChangeInterface &&) = delete; + ~LaneChangeInterface() override = default; + + void processOnEntry() override; + + void processOnExit() override; + + bool isExecutionRequested() const override; + + bool isExecutionReady() const override; + + bool isCurrentRouteLaneletToBeReset() const override + { + return getCurrentStatus() == ModuleStatus::SUCCESS; + } + + void updateData() override; + + void postProcess() override; + + BehaviorModuleOutput plan() override; + + BehaviorModuleOutput planWaitingApproval() override; + + CandidateOutput planCandidate() const override; + + void acceptVisitor( + [[maybe_unused]] const std::shared_ptr & visitor) const override + { + } + + void updateModuleParams(const std::any & parameters) override; + + void setData(const std::shared_ptr & data) override; + + MarkerArray getModuleVirtualWall() override; + + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override + { + updateData(); + + if (!isWaitingApproval()) { + return plan(); + } + + // module is waiting approval. Check it. + if (isActivated()) { + RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); + return plan(); + } else { + RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); + return planWaitingApproval(); + } + } + +protected: + std::shared_ptr parameters_; + + std::unique_ptr module_type_; + + PathSafetyStatus post_process_safety_status_; + + bool canTransitSuccessState() override; + + bool canTransitFailureState() override; + + ModuleStatus setInitState() const override { return ModuleStatus::WAITING_APPROVAL; }; + + void updateRTCStatus( + const double start_distance, const double finish_distance, const bool safe, + const uint8_t & state) + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), safe, state, start_distance, finish_distance, clock_->now()); + } + } + } + + void updateDebugMarker() const; + + void updateSteeringFactorPtr(const BehaviorModuleOutput & output); + + void updateSteeringFactorPtr( + const CandidateOutput & output, const LaneChangePath & selected_path) const; + + mutable MarkerArray virtual_wall_marker_; + + std::unique_ptr prev_approved_path_; + + void clearAbortApproval() { is_abort_path_approved_ = false; } + + bool is_abort_path_approved_{false}; + + bool is_abort_approval_requested_{false}; +}; +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/manager.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/manager.hpp new file mode 100644 index 0000000000000..e614a9a084a14 --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/manager.hpp @@ -0,0 +1,76 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ + +#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_route_handler/route_handler.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::route_handler::Direction; + +class LaneChangeModuleManager : public SceneModuleManagerInterface +{ +public: + LaneChangeModuleManager( + const std::string & name, const Direction direction, const LaneChangeModuleType type) + : SceneModuleManagerInterface{name}, direction_{direction}, type_{type} + { + } + + void init(rclcpp::Node * node) override; + + std::unique_ptr createNewSceneModuleInstance() override; + + void updateModuleParams(const std::vector & parameters) override; + +protected: + void initParams(rclcpp::Node * node); + + std::shared_ptr parameters_; + + Direction direction_; + + LaneChangeModuleType type_; +}; + +class LaneChangeRightModuleManager : public LaneChangeModuleManager +{ +public: + LaneChangeRightModuleManager() + : LaneChangeModuleManager("lane_change_right", Direction::RIGHT, LaneChangeModuleType::NORMAL) + { + } +}; + +class LaneChangeLeftModuleManager : public LaneChangeModuleManager +{ +public: + LaneChangeLeftModuleManager() + : LaneChangeModuleManager("lane_change_left", Direction::LEFT, LaneChangeModuleType::NORMAL) + { + } +}; +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/scene.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/scene.hpp new file mode 100644 index 0000000000000..5391f75adfd46 --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/scene.hpp @@ -0,0 +1,209 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ + +#include "autoware_behavior_path_lane_change_module/utils/base_class.hpp" +#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using autoware::behavior_path_planner::utils::path_safety_checker:: + PoseWithVelocityAndPolygonStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; +using autoware::route_handler::Direction; +using data::lane_change::LanesPolygon; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::PathWithLaneId; +using utils::path_safety_checker::ExtendedPredictedObjects; + +class NormalLaneChange : public LaneChangeBase +{ +public: + NormalLaneChange( + const std::shared_ptr & parameters, LaneChangeModuleType type, + Direction direction); + + NormalLaneChange(const NormalLaneChange &) = delete; + NormalLaneChange(NormalLaneChange &&) = delete; + NormalLaneChange & operator=(const NormalLaneChange &) = delete; + NormalLaneChange & operator=(NormalLaneChange &&) = delete; + ~NormalLaneChange() override = default; + + void updateLaneChangeStatus() override; + + std::pair getSafePath(LaneChangePath & safe_path) const override; + + LaneChangePath getLaneChangePath() const override; + + BehaviorModuleOutput getTerminalLaneChangePath() const override; + + BehaviorModuleOutput generateOutput() override; + + void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; + + void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + + PathWithLaneId getReferencePath() const override; + + std::optional extendPath() override; + + void resetParameters() override; + + TurnSignalInfo updateOutputTurnSignal() const override; + + bool calcAbortPath() override; + + PathSafetyStatus isApprovedPathSafe() const override; + + PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( + PathSafetyStatus approved_path_safety_status) override; + + bool isRequiredStop(const bool is_object_coming_from_rear) override; + + bool isNearEndOfCurrentLanes( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const double threshold) const override; + + bool hasFinishedLaneChange() const override; + + bool isAbleToReturnCurrentLane() const override; + + bool isEgoOnPreparePhase() const override; + + bool isAbleToStopSafely() const override; + + bool hasFinishedAbort() const override; + + bool isAbortState() const override; + + bool isLaneChangeRequired() override; + + bool isStoppedAtRedTrafficLight() const override; + + TurnSignalInfo get_current_turn_signal_info() override; + +protected: + lanelet::ConstLanelets getCurrentLanes() const override; + + lanelet::ConstLanelets getLaneChangeLanes( + const lanelet::ConstLanelets & current_lanes, Direction direction) const override; + + int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + + std::vector sampleLongitudinalAccValues( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + + std::vector calcPrepareDuration( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + + ExtendedPredictedObjects getTargetObjects( + const LaneChangeLanesFilteredObjects & predicted_objects, + const lanelet::ConstLanelets & current_lanes) const; + + LaneChangeLanesFilteredObjects filterObjects( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + + void filterOncomingObjects(PredictedObjects & objects) const; + + void filterAheadTerminalObjects( + PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const; + + void filterObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, + std::vector & current_lane_objects, + std::vector & target_lane_objects, + std::vector & other_lane_objects) const; + + PathWithLaneId getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double backward_path_length, + const double prepare_length) const override; + + PathWithLaneId getTargetSegment( + const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, + const double target_lane_length, const double lane_changing_length, + const double lane_changing_velocity, const double buffer_for_next_lane_change) const; + + bool hasEnoughLength( + 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 hasEnoughLengthToTrafficLight( + 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; + + std::optional calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + + bool isValidPath(const PathWithLaneId & path) const override; + + PathSafetyStatus isLaneChangePathSafe( + const LaneChangePath & lane_change_path, + const ExtendedPredictedObjects & collision_check_objects, + const utils::path_safety_checker::RSSparams & rss_params, + CollisionCheckDebugMap & debug_data) const; + + //! @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; + + double get_max_velocity_for_safety_check() const; + + bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + + bool check_prepare_phase() 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 autoware::behavior_path_planner +#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp similarity index 89% rename from planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp rename to planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp index 3f241b9d6598d..f1b7e6308313d 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp @@ -11,16 +11,16 @@ // 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_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ -#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ - -#include "behavior_path_lane_change_module/utils/data_structs.hpp" -#include "behavior_path_lane_change_module/utils/debug_structs.hpp" -#include "behavior_path_lane_change_module/utils/path.hpp" -#include "behavior_path_lane_change_module/utils/utils.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/turn_signal_decider.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ + +#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware_behavior_path_lane_change_module/utils/debug_structs.hpp" +#include "autoware_behavior_path_lane_change_module/utils/path.hpp" +#include "autoware_behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include @@ -34,13 +34,13 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using autoware::route_handler::Direction; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using route_handler::Direction; using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::PathWithLaneId; @@ -238,5 +238,5 @@ class LaneChangeBase rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr()); mutable rclcpp::Clock clock_{RCL_ROS_TIME}; }; -} // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp new file mode 100644 index 0000000000000..0307799ff7bdc --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp @@ -0,0 +1,229 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ + +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + +#include + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ +struct LateralAccelerationMap +{ + std::vector base_vel; + std::vector base_min_acc; + std::vector base_max_acc; + + void add(const double velocity, const double min_acc, const double max_acc) + { + if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { + return; + } + + size_t idx = 0; + for (size_t i = 0; i < base_vel.size(); ++i) { + if (velocity < base_vel.at(i)) { + break; + } + idx = i + 1; + } + + base_vel.insert(base_vel.begin() + idx, velocity); + base_min_acc.insert(base_min_acc.begin() + idx, min_acc); + base_max_acc.insert(base_max_acc.begin() + idx, max_acc); + } + + std::pair find(const double velocity) const + { + if (!base_vel.empty() && velocity < base_vel.front()) { + return std::make_pair(base_min_acc.front(), base_max_acc.front()); + } + if (!base_vel.empty() && velocity > base_vel.back()) { + return std::make_pair(base_min_acc.back(), base_max_acc.back()); + } + + const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); + + return std::make_pair(min_acc, max_acc); + } +}; + +struct LaneChangeCancelParameters +{ + bool enable_on_prepare_phase{true}; + bool enable_on_lane_changing_phase{false}; + double delta_time{1.0}; + double duration{5.0}; + double max_lateral_jerk{10.0}; + double overhang_tolerance{0.0}; + + // unsafe_hysteresis_threshold will be compare with the number of detected unsafe instance. If the + // number of unsafe exceeds unsafe_hysteresis_threshold, the lane change will be cancelled or + // aborted. + int unsafe_hysteresis_threshold{2}; +}; + +struct LaneChangeParameters +{ + // trajectory generation + double backward_lane_length{200.0}; + double prediction_time_resolution{0.5}; + int longitudinal_acc_sampling_num{10}; + int lateral_acc_sampling_num{10}; + + // lane change parameters + double backward_length_buffer_for_end_of_lane; + double backward_length_buffer_for_blocking_object; + double lane_changing_lateral_jerk{0.5}; + double minimum_lane_changing_velocity{5.6}; + double lane_change_prepare_duration{4.0}; + double lane_change_finish_judge_buffer{3.0}; + LateralAccelerationMap lane_change_lat_acc_map; + + // parked vehicle + double object_check_min_road_shoulder_width{0.5}; + double object_shiftable_ratio_threshold{0.6}; + + // turn signal + double min_length_for_turn_signal_activation{10.0}; + double length_ratio_for_turn_signal_deactivation{0.8}; + + // acceleration data + double min_longitudinal_acc{-1.0}; + double max_longitudinal_acc{1.0}; + + // collision check + bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; + bool enable_collision_check_for_prepare_phase_in_intersection{true}; + bool enable_collision_check_for_prepare_phase_in_turns{true}; + double prepare_segment_ignore_object_velocity_thresh{0.1}; + 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}; + + // regulatory elements + bool regulate_on_crosswalk{false}; + bool regulate_on_intersection{false}; + bool regulate_on_traffic_light{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 + 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{}; + + double finish_judge_lateral_threshold{0.2}; + + // debug marker + bool publish_debug_marker{false}; +}; + +enum class LaneChangeStates { + Normal = 0, + Cancel, + Abort, + Stop, +}; + +struct LaneChangePhaseInfo +{ + double prepare{0.0}; + double lane_changing{0.0}; + + [[nodiscard]] double sum() const { return prepare + lane_changing; } + + LaneChangePhaseInfo(const double _prepare, const double _lane_changing) + : prepare(_prepare), lane_changing(_lane_changing) + { + } +}; + +struct LaneChangeInfo +{ + LaneChangePhaseInfo longitudinal_acceleration{0.0, 0.0}; + LaneChangePhaseInfo velocity{0.0, 0.0}; + LaneChangePhaseInfo duration{0.0, 0.0}; + LaneChangePhaseInfo length{0.0, 0.0}; + + lanelet::ConstLanelets current_lanes{}; + lanelet::ConstLanelets target_lanes{}; + + Pose lane_changing_start{}; + Pose lane_changing_end{}; + + ShiftLine shift_line{}; + + double lateral_acceleration{0.0}; + double terminal_lane_changing_velocity{0.0}; +}; + +struct LaneChangeTargetObjectIndices +{ + std::vector current_lane{}; + std::vector target_lane{}; + std::vector other_lane{}; +}; + +struct LaneChangeLanesFilteredObjects +{ + utils::path_safety_checker::ExtendedPredictedObjects current_lane{}; + utils::path_safety_checker::ExtendedPredictedObjects target_lane{}; + utils::path_safety_checker::ExtendedPredictedObjects other_lane{}; +}; + +enum class LaneChangeModuleType { + NORMAL = 0, + EXTERNAL_REQUEST, + AVOIDANCE_BY_LANE_CHANGE, +}; +} // namespace autoware::behavior_path_planner + +namespace autoware::behavior_path_planner::data::lane_change +{ +struct PathSafetyStatus +{ + bool is_safe{true}; + bool is_object_coming_from_rear{false}; +}; + +struct LanesPolygon +{ + std::optional current; + std::optional target; + std::vector target_backward; +}; +} // namespace autoware::behavior_path_planner::data::lane_change + +#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/debug_structs.hpp similarity index 79% rename from planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp rename to planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/debug_structs.hpp index fbdd742a8457b..14b900b2fa826 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/debug_structs.hpp @@ -11,13 +11,13 @@ // 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_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ -#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ -#include "behavior_path_lane_change_module/utils/data_structs.hpp" -#include "behavior_path_lane_change_module/utils/path.hpp" +#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware_behavior_path_lane_change_module/utils/path.hpp" -#include +#include #include @@ -26,7 +26,7 @@ #include #include -namespace behavior_path_planner::data::lane_change +namespace autoware::behavior_path_planner::data::lane_change { using utils::path_safety_checker::CollisionCheckDebugMap; struct Debug @@ -71,6 +71,6 @@ struct Debug is_abort = false; } }; -} // namespace behavior_path_planner::data::lane_change +} // namespace autoware::behavior_path_planner::data::lane_change -#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/markers.hpp similarity index 75% rename from planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp rename to planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/markers.hpp index bc4413e1c69d3..87724bb25a7b5 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/markers.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ -#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ -#include "behavior_path_lane_change_module/utils/debug_structs.hpp" -#include "behavior_path_lane_change_module/utils/path.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_lane_change_module/utils/debug_structs.hpp" +#include "autoware_behavior_path_lane_change_module/utils/path.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -29,9 +29,9 @@ namespace marker_utils::lane_change_markers { -using behavior_path_planner::LaneChangePath; -using behavior_path_planner::data::lane_change::Debug; -using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; +using autoware::behavior_path_planner::LaneChangePath; +using autoware::behavior_path_planner::data::lane_change::Debug; +using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( const std::vector & lanes, std::string && ns); @@ -48,4 +48,4 @@ MarkerArray createDebugMarkerArray( const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); } // namespace marker_utils::lane_change_markers -#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/path.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/path.hpp new file mode 100644 index 0000000000000..28f4925eebdd1 --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/path.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ + +#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + +#include + +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::TurnSignalInfo; +using tier4_planning_msgs::msg::PathWithLaneId; +struct LaneChangePath +{ + PathWithLaneId path{}; + ShiftedPath shifted_path{}; + PathWithLaneId prev_path{}; + LaneChangeInfo info{}; +}; +using LaneChangePaths = std::vector; + +struct LaneChangeStatus +{ + PathWithLaneId lane_follow_path{}; + LaneChangePath lane_change_path{}; + lanelet::ConstLanelets current_lanes{}; + lanelet::ConstLanelets target_lanes{}; + std::vector lane_follow_lane_ids{}; + std::vector lane_change_lane_ids{}; + bool is_safe{false}; + bool is_valid_path{false}; + double start_distance{0.0}; +}; + +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp new file mode 100644 index 0000000000000..1fd716dc74c7c --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp @@ -0,0 +1,312 @@ +// Copyright 2021 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ + +#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware_behavior_path_lane_change_module/utils/path.hpp" +#include "autoware_behavior_path_planner_common/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "rclcpp/logger.hpp" + +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner::utils::lane_change +{ +using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using autoware::behavior_path_planner::utils::path_safety_checker:: + PoseWithVelocityAndPolygonStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; +using autoware::route_handler::Direction; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using data::lane_change::LanesPolygon; +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 tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; + +double calcLaneChangeResampleInterval( + const double lane_changing_length, const double lane_changing_velocity); + +double calcMinimumLaneChangeLength( + const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals); + +double calcMinimumLaneChangeLength( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, + const LaneChangeParameters & lane_change_parameters, Direction direction); + +double calcMaximumLaneChangeLength( + const double current_velocity, const LaneChangeParameters & lane_change_parameters, + const std::vector & shift_intervals, const double max_acc); + +double calcMinimumAcceleration( + const double current_velocity, const double min_longitudinal_acc, + const LaneChangeParameters & lane_change_parameters); + +double calcMaximumAcceleration( + const double current_velocity, const double current_max_velocity, + const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); + +double calcLaneChangingAcceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc); + +void setPrepareVelocity( + PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); + +std::vector getAccelerationValues( + const double min_acc, const double max_acc, const size_t sampling_num); + +std::vector replaceWithSortedIds( + const std::vector & original_lane_ids, + const std::vector> & sorted_lane_ids); + +std::vector> getSortedLaneIds( + const RouteHandler & route_handler, const Pose & current_pose, + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); + +lanelet::ConstLanelets getTargetPreferredLanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const Direction & direction, + const LaneChangeModuleType & type); + +lanelet::ConstLanelets getTargetNeighborLanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const LaneChangeModuleType & type); + +lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType & type); + +bool isPathInLanelets( + const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes); + +std::optional constructCandidatePath( + const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, + const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, + const std::vector> & sorted_lane_ids); + +ShiftLine getLaneChangingShiftLine( + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const PathWithLaneId & reference_path, const double shift_length); + +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length); + +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & lane_changing_start_pose, const double target_lane_length, + const double lane_changing_length, const double forward_path_length, + const double resample_interval, const bool is_goal_in_route, + const double next_lane_change_buffer); + +std::vector generateDrivableLanes( + const std::vector original_drivable_lanes, const RouteHandler & route_handler, + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); + +std::vector generateDrivableLanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & lane_change_lanes); + +double getLateralShift(const LaneChangePath & path); + +bool hasEnoughLengthToLaneChangeAfterAbort( + const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, + const Pose & curent_pose, const double abort_return_dist, + const LaneChangeParameters & lane_change_parameters, const Direction direction); + +double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); + +double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer); + +std::string getStrDirection(const std::string & name, const Direction direction); + +CandidateOutput assignToCandidate( + const LaneChangePath & lane_change_path, const Point & ego_position); +std::optional getLaneChangeTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType type, const Direction & direction); + +std::vector convertToPredictedPath( + const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const double resolution); + +bool isParkedObject( + const PathWithLaneId & path, const RouteHandler & route_handler, + const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, + const double object_shiftable_ratio_threshold, + const double static_object_velocity_threshold = 1.0); + +bool isParkedObject( + const lanelet::ConstLanelet & closest_lanelet, const lanelet::BasicLineString2d & boundary, + const ExtendedPredictedObject & object, const double buffer_to_bound, + const double ratio_threshold); + +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, + CollisionCheckDebugMap & object_debug); + +std::optional getLeadingStaticObjectIdx( + const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const std::vector & objects, + const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); + +std::optional createPolygon( + const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); + +ExtendedPredictedObject transform( + const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); + +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); + +/** + * @brief Retrieves a logger instance for a specific lane change type. + * + * This function provides a specialized logger for different types of lane change. + * + * @param type A string representing the type of lane change operation. This could be + * a specific maneuver or condition related to lane changing, such as + * 'avoidance_by_lane_change', 'normal', 'external_request'. + * + * @return rclcpp::Logger The logger instance configured for the specified lane change type. + */ +rclcpp::Logger getLogger(const std::string & type); + +/** + * @brief Computes the current footprint of the ego vehicle based on its pose and size. + * + * This function calculates the 2D polygon representing the current footprint of the ego vehicle. + * The footprint is determined by the vehicle's pose and its dimensions, including the distance + * from the base to the front and rear ends of the vehicle, as well as its width. + * + * @param ego_pose The current pose of the ego vehicle. + * @param ego_info The structural information of the ego vehicle, such as its maximum longitudinal + * offset, rear overhang, and width. + * + * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. + */ +Polygon2d getEgoCurrentFootprint( + const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info); + +/** + * @brief Checks if the given polygon is within an intersection area. + * + * This function evaluates whether a specified polygon is located within the bounds of an + * intersection. It identifies the intersection area by checking the attributes of the provided + * lanelet. If the lanelet has an attribute indicating it is part of an intersection, the function + * then checks if the polygon is fully contained within this area. + * + * @param route_handler a shared pointer to the route_handler + * @param lanelet A lanelet to check against the + * intersection area. + * @param polygon The polygon to check for containment within the intersection area. + * + * @return bool True if the polygon is within the intersection area, false otherwise. + */ +bool isWithinIntersection( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, + const Polygon2d & polygon); + +/** + * @brief Determines if a polygon is within lanes designated for turning. + * + * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding 'straight'). + * It evaluates the lanelet's 'turn_direction' attribute and determines overlap with the lanelet's + * area. + * + * @param lanelet Lanelet representing the road segment whose turn direction is to be evaluated. + * @param polygon The polygon to be checked for its presence within turn direction lanes. + * + * @return bool True if the polygon is within a lane designated for turning, false if it is within a + * straight lane or no turn direction is specified. + */ +bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); + +/** + * @brief Calculates the distance required during a lane change operation. + * + * Used for computing prepare or lane change length based on current and maximum velocity, + * acceleration, and duration, returning the lesser of accelerated distance or distance at max + * velocity. + * + * @param velocity The current velocity of the vehicle in meters per second (m/s). + * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). + * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). + * @param duration The duration of the lane change in seconds (s). + * @return The calculated minimum distance in meters (m). + */ +double calcPhaseLength( + const double velocity, const double maximum_velocity, const double acceleration, + const double time); + +LanesPolygon createLanesPolygon( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const std::vector & target_backward_lanes); +} // namespace autoware::behavior_path_planner::utils::lane_change + +namespace autoware::behavior_path_planner::utils::lane_change::debug +{ +geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); + +geometry_msgs::msg::Polygon createExecutionArea( + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, + double additional_lon_offset, double additional_lat_offset); +} // namespace autoware::behavior_path_planner::utils::lane_change::debug + +#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/package.xml b/planning/autoware_behavior_path_lane_change_module/package.xml new file mode 100644 index 0000000000000..dca36f99e3faa --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/package.xml @@ -0,0 +1,39 @@ + + + + autoware_behavior_path_lane_change_module + 0.1.0 + The autoware_behavior_path_lane_change_module package + + Fumiya Watanabe + Satoshi Ota + Zulfaqar Azmi + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_path_planner + autoware_behavior_path_planner_common + autoware_rtc_interface + motion_utils + pluginlib + rclcpp + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_path_lane_change_module/plugins.xml b/planning/autoware_behavior_path_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..3ea66502fe6fc --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/autoware_behavior_path_lane_change_module/src/interface.cpp new file mode 100644 index 0000000000000..888594e0c41e5 --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -0,0 +1,383 @@ +// 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 "autoware_behavior_path_lane_change_module/interface.hpp" + +#include "autoware_behavior_path_lane_change_module/utils/markers.hpp" +#include "autoware_behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using utils::lane_change::assignToCandidate; + +LaneChangeInterface::LaneChangeInterface( + const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map, + std::unique_ptr && module_type) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + parameters_{std::move(parameters)}, + module_type_{std::move(module_type)}, + prev_approved_path_{std::make_unique()} +{ + steering_factor_interface_ptr_ = std::make_unique(&node, name); + logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); +} + +void LaneChangeInterface::processOnEntry() +{ + waitApproval(); +} + +void LaneChangeInterface::processOnExit() +{ + module_type_->resetParameters(); + debug_marker_.markers.clear(); + post_process_safety_status_ = {}; + resetPathCandidate(); +} + +bool LaneChangeInterface::isExecutionRequested() const +{ + if (getCurrentStatus() == ModuleStatus::RUNNING) { + return true; + } + + return module_type_->isLaneChangeRequired(); +} + +bool LaneChangeInterface::isExecutionReady() const +{ + return module_type_->isSafe() && !module_type_->isAbortState(); +} + +void LaneChangeInterface::updateData() +{ + module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); + module_type_->updateSpecialData(); + + if (isWaitingApproval()) { + module_type_->updateLaneChangeStatus(); + } + + module_type_->resetStopPose(); + updateDebugMarker(); +} + +void LaneChangeInterface::postProcess() +{ + if (getCurrentStatus() == ModuleStatus::RUNNING) { + const auto safety_status = module_type_->isApprovedPathSafe(); + post_process_safety_status_ = + module_type_->evaluateApprovedPathWithUnsafeHysteresis(safety_status); + } + updateDebugMarker(); +} + +BehaviorModuleOutput LaneChangeInterface::plan() +{ + resetPathCandidate(); + resetPathReference(); + + auto output = module_type_->generateOutput(); + path_reference_ = std::make_shared(output.reference_path); + *prev_approved_path_ = getPreviousModuleOutput().path; + + stop_pose_ = module_type_->getStopPose(); + + const auto & lane_change_debug = module_type_->getDebugData(); + for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } + + updateSteeringFactorPtr(output); + if (module_type_->isAbortState()) { + waitApproval(); + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::ABORTING); + } else { + clearWaitingApproval(); + const auto path = + assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition()); + updateRTCStatus( + path.start_distance_to_path_change, path.finish_distance_to_path_change, true, + State::RUNNING); + } + + return output; +} + +BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() +{ + *prev_approved_path_ = getPreviousModuleOutput().path; + + BehaviorModuleOutput out = getPreviousModuleOutput(); + module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + out.turn_signal_info = module_type_->get_current_turn_signal_info(); + + const auto & lane_change_debug = module_type_->getDebugData(); + for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } + + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); + stop_pose_ = module_type_->getStopPose(); + + if (!module_type_->isValidPath()) { + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); + path_candidate_ = std::make_shared(); + return out; + } + + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + + updateRTCStatus( + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change, + isExecutionReady(), State::WAITING_FOR_EXECUTION); + is_abort_path_approved_ = false; + + return out; +} + +CandidateOutput LaneChangeInterface::planCandidate() const +{ + const auto selected_path = module_type_->getLaneChangePath(); + + if (selected_path.path.points.empty()) { + return {}; + } + + CandidateOutput output = assignToCandidate(selected_path, module_type_->getEgoPosition()); + + updateSteeringFactorPtr(output, selected_path); + return output; +} + +void LaneChangeInterface::updateModuleParams(const std::any & parameters) +{ + parameters_ = std::any_cast>(parameters); +} + +void LaneChangeInterface::setData(const std::shared_ptr & data) +{ + planner_data_ = data; + module_type_->setData(data); +} + +bool LaneChangeInterface::canTransitSuccessState() +{ + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + updateDebugMarker(); + + if (module_type_->specialExpiredCheck() && isWaitingApproval()) { + log_debug_throttled("Run specialExpiredCheck."); + if (isWaitingApproval()) { + return true; + } + } + + if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { + log_debug_throttled("Abort process has completed."); + return true; + } + + if (module_type_->hasFinishedLaneChange()) { + module_type_->resetParameters(); + log_debug_throttled("Lane change process has completed."); + return true; + } + + log_debug_throttled("Lane changing process is ongoing"); + return false; +} + +bool LaneChangeInterface::canTransitFailureState() +{ + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + + updateDebugMarker(); + log_debug_throttled(__func__); + + if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { + log_debug_throttled("Abort process has on going."); + return false; + } + + if (isWaitingApproval()) { + log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL"); + return false; + } + + if (module_type_->isCancelEnabled() && module_type_->isEgoOnPreparePhase()) { + if (module_type_->isStoppedAtRedTrafficLight()) { + log_debug_throttled("Stopping at traffic light while in prepare phase. Cancel lane change"); + module_type_->toCancelState(); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); + return true; + } + + if (post_process_safety_status_.is_safe) { + log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe."); + return false; + } + + if (module_type_->isAbleToReturnCurrentLane()) { + log_debug_throttled("It's possible to return to current lane. Cancel lane change."); + updateRTCStatus( + std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, + State::FAILED); + return true; + } + } + + if (post_process_safety_status_.is_safe) { + log_debug_throttled("Can't transit to failure state. Ego is lane changing, and it's safe."); + return false; + } + + if (module_type_->isRequiredStop(post_process_safety_status_.is_object_coming_from_rear)) { + log_debug_throttled("Module require stopping"); + } + + if (!module_type_->isCancelEnabled()) { + log_debug_throttled( + "Lane change path is unsafe but cancel was not enabled. Continue lane change."); + return false; + } + + if (!module_type_->isAbortEnabled()) { + log_debug_throttled( + "Lane change path is unsafe but abort was not enabled. Continue lane change."); + return false; + } + + if (!module_type_->isAbleToReturnCurrentLane()) { + log_debug_throttled("It's is not possible to return to original lane. Continue lane change."); + return false; + } + + const auto found_abort_path = module_type_->calcAbortPath(); + if (!found_abort_path) { + log_debug_throttled( + "Lane change path is unsafe but abort path not found. Continue lane change."); + return false; + } + + log_debug_throttled("Lane change path is unsafe. Abort lane change."); + module_type_->toAbortState(); + return false; +} + +void LaneChangeInterface::updateDebugMarker() const +{ + debug_marker_.markers.clear(); + if (!parameters_->publish_debug_marker) { + return; + } + using marker_utils::lane_change_markers::createDebugMarkerArray; + debug_marker_ = createDebugMarkerArray(module_type_->getDebugData(), module_type_->getEgoPose()); +} + +MarkerArray LaneChangeInterface::getModuleVirtualWall() +{ + using marker_utils::lane_change_markers::createLaneChangingVirtualWallMarker; + MarkerArray marker; + + if (!parameters_->publish_debug_marker) { + return marker; + } + + if (isWaitingApproval() || getCurrentStatus() != ModuleStatus::RUNNING) { + return marker; + } + const auto & start_pose = module_type_->getLaneChangePath().info.lane_changing_start; + const auto start_marker = + createLaneChangingVirtualWallMarker(start_pose, name(), clock_->now(), "lane_change_start"); + + const auto & end_pose = module_type_->getLaneChangePath().info.lane_changing_end; + const auto end_marker = + createLaneChangingVirtualWallMarker(end_pose, name(), clock_->now(), "lane_change_end"); + marker.markers.reserve(start_marker.markers.size() + end_marker.markers.size()); + appendMarkerArray(start_marker, &marker); + appendMarkerArray(end_marker, &marker); + return marker; +} + +void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output) +{ + const auto steering_factor_direction = std::invoke([&]() { + if (module_type_->getDirection() == Direction::LEFT) { + return SteeringFactor::LEFT; + } + if (module_type_->getDirection() == Direction::RIGHT) { + return SteeringFactor::RIGHT; + } + return SteeringFactor::UNKNOWN; + }); + + const auto current_position = module_type_->getEgoPosition(); + const auto status = module_type_->getLaneChangeStatus(); + const auto start_distance = motion_utils::calcSignedArcLength( + output.path.points, current_position, status.lane_change_path.info.shift_line.start.position); + const auto finish_distance = motion_utils::calcSignedArcLength( + output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); + + steering_factor_interface_ptr_->updateSteeringFactor( + {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, + {start_distance, finish_distance}, PlanningBehavior::LANE_CHANGE, steering_factor_direction, + SteeringFactor::TURNING, ""); +} + +void LaneChangeInterface::updateSteeringFactorPtr( + const CandidateOutput & output, const LaneChangePath & selected_path) const +{ + const uint16_t steering_factor_direction = std::invoke([&output]() { + if (output.lateral_shift > 0.0) { + return SteeringFactor::LEFT; + } + return SteeringFactor::RIGHT; + }); + + steering_factor_interface_ptr_->updateSteeringFactor( + {selected_path.info.shift_line.start, selected_path.info.shift_line.end}, + {output.start_distance_to_path_change, output.finish_distance_to_path_change}, + PlanningBehavior::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); +} +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/autoware_behavior_path_lane_change_module/src/manager.cpp new file mode 100644 index 0000000000000..8a359df98f04f --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -0,0 +1,401 @@ +// 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 "autoware_behavior_path_lane_change_module/manager.hpp" + +#include "autoware_behavior_path_lane_change_module/interface.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ + +void LaneChangeModuleManager::init(rclcpp::Node * node) +{ + // init manager interface + initInterface(node, {""}); + initParams(node); +} + +void LaneChangeModuleManager::initParams(rclcpp::Node * node) +{ + using tier4_autoware_utils::getOrDeclareParameter; + + LaneChangeParameters p{}; + + const auto parameter = [](std::string && name) { return "lane_change." + name; }; + + // trajectory generation + p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); + p.prediction_time_resolution = + getOrDeclareParameter(*node, parameter("prediction_time_resolution")); + p.longitudinal_acc_sampling_num = + getOrDeclareParameter(*node, parameter("longitudinal_acceleration_sampling_num")); + p.lateral_acc_sampling_num = + getOrDeclareParameter(*node, parameter("lateral_acceleration_sampling_num")); + + // parked vehicle detection + p.object_check_min_road_shoulder_width = + getOrDeclareParameter(*node, parameter("object_check_min_road_shoulder_width")); + p.object_shiftable_ratio_threshold = + getOrDeclareParameter(*node, parameter("object_shiftable_ratio_threshold")); + + // turn signal + p.min_length_for_turn_signal_activation = + getOrDeclareParameter(*node, parameter("min_length_for_turn_signal_activation")); + p.length_ratio_for_turn_signal_deactivation = + getOrDeclareParameter(*node, parameter("length_ratio_for_turn_signal_deactivation")); + + // acceleration + p.min_longitudinal_acc = getOrDeclareParameter(*node, parameter("min_longitudinal_acc")); + p.max_longitudinal_acc = getOrDeclareParameter(*node, parameter("max_longitudinal_acc")); + + // collision check + p.enable_collision_check_for_prepare_phase_in_general_lanes = getOrDeclareParameter( + *node, parameter("enable_collision_check_for_prepare_phase.general_lanes")); + p.enable_collision_check_for_prepare_phase_in_intersection = getOrDeclareParameter( + *node, parameter("enable_collision_check_for_prepare_phase.intersection")); + p.enable_collision_check_for_prepare_phase_in_turns = + getOrDeclareParameter(*node, parameter("enable_collision_check_for_prepare_phase.turns")); + p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter( + *node, parameter("prepare_segment_ignore_object_velocity_thresh")); + p.check_objects_on_current_lanes = + getOrDeclareParameter(*node, parameter("check_objects_on_current_lanes")); + p.check_objects_on_other_lanes = + 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")); + p.regulate_on_traffic_light = + getOrDeclareParameter(*node, parameter("regulation.traffic_light")); + + // 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")); + + // lane change parameters + const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + p.backward_length_buffer_for_end_of_lane = + getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); + p.backward_length_buffer_for_blocking_object = + getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); + p.lane_changing_lateral_jerk = + getOrDeclareParameter(*node, parameter("lane_changing_lateral_jerk")); + p.lane_change_prepare_duration = + getOrDeclareParameter(*node, parameter("prepare_duration")); + p.minimum_lane_changing_velocity = + getOrDeclareParameter(*node, parameter("minimum_lane_changing_velocity")); + p.minimum_lane_changing_velocity = + std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration); + p.lane_change_finish_judge_buffer = + getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); + + if (p.backward_length_buffer_for_end_of_lane < 1.0) { + RCLCPP_WARN_STREAM( + node->get_logger().get_child(name()), + "Lane change buffer must be more than 1 meter. Modifying the buffer."); + } + + // lateral acceleration map for lane change + const auto lateral_acc_velocity = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); + const auto min_lateral_acc = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.min_values")); + const auto max_lateral_acc = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.max_values")); + if ( + lateral_acc_velocity.size() != min_lateral_acc.size() || + lateral_acc_velocity.size() != max_lateral_acc.size()) { + RCLCPP_ERROR( + node->get_logger().get_child(name()), + "Lane change lateral acceleration map has invalid size."); + exit(EXIT_FAILURE); + } + for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { + p.lane_change_lat_acc_map.add( + lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); + } + + // target object + { + const std::string ns = "lane_change.target_object."; + 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 = + getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); + p.cancel.enable_on_lane_changing_phase = + 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.cancel.unsafe_hysteresis_threshold = + getOrDeclareParameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); + + p.finish_judge_lateral_threshold = + getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); + + // 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) { + RCLCPP_FATAL_STREAM( + node->get_logger().get_child(name()), + "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " + << p.longitudinal_acc_sampling_num + << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl + << "Terminating the program..."); + 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( + node->get_logger().get_child(name()), + "abort parameter might be loose... Terminating the program..."); + exit(EXIT_FAILURE); + } + } + if (p.cancel.delta_time < 1.0) { + RCLCPP_WARN_STREAM( + node->get_logger().get_child(name()), + "cancel.delta_time: " << p.cancel.delta_time + << ", is too short. This could cause a danger behavior."); + } + + parameters_ = std::make_shared(p); +} + +std::unique_ptr LaneChangeModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, + std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); +} + +void LaneChangeModuleManager::updateModuleParams(const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + auto p = parameters_; + + { + const std::string ns = "lane_change."; + updateParam(parameters, ns + "backward_lane_length", p->backward_lane_length); + updateParam(parameters, ns + "prepare_duration", p->lane_change_prepare_duration); + + updateParam( + parameters, ns + "backward_length_buffer_for_end_of_lane", + p->backward_length_buffer_for_end_of_lane); + updateParam( + parameters, ns + "backward_length_buffer_for_blocking_object", + p->backward_length_buffer_for_blocking_object); + updateParam( + parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer); + + updateParam( + parameters, ns + "lane_changing_lateral_jerk", p->lane_changing_lateral_jerk); + + updateParam( + parameters, ns + "minimum_lane_changing_velocity", p->minimum_lane_changing_velocity); + updateParam( + parameters, ns + "prediction_time_resolution", p->prediction_time_resolution); + + int longitudinal_acc_sampling_num = 0; + updateParam( + parameters, ns + "longitudinal_acceleration_sampling_num", longitudinal_acc_sampling_num); + if (longitudinal_acc_sampling_num > 0) { + p->longitudinal_acc_sampling_num = longitudinal_acc_sampling_num; + } + + int lateral_acc_sampling_num = 0; + updateParam( + parameters, ns + "lateral_acceleration_sampling_num", lateral_acc_sampling_num); + if (lateral_acc_sampling_num > 0) { + p->lateral_acc_sampling_num = lateral_acc_sampling_num; + } + + updateParam( + parameters, ns + "finish_judge_lateral_threshold", p->finish_judge_lateral_threshold); + updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + + // longitudinal acceleration + updateParam(parameters, ns + "min_longitudinal_acc", p->min_longitudinal_acc); + updateParam(parameters, ns + "max_longitudinal_acc", p->max_longitudinal_acc); + } + + { + const std::string ns = "lane_change.safety_check.lane_expansion."; + updateParam(parameters, ns + "left_offset", p->lane_expansion_left_offset); + updateParam(parameters, ns + "right_offset", p->lane_expansion_right_offset); + } + + { + const std::string ns = "lane_change.target_object."; + updateParam(parameters, ns + "car", p->object_types_to_check.check_car); + updateParam(parameters, ns + "truck", p->object_types_to_check.check_truck); + updateParam(parameters, ns + "bus", p->object_types_to_check.check_bus); + updateParam(parameters, ns + "trailer", p->object_types_to_check.check_trailer); + updateParam(parameters, ns + "unknown", p->object_types_to_check.check_unknown); + updateParam(parameters, ns + "bicycle", p->object_types_to_check.check_bicycle); + updateParam(parameters, ns + "motorcycle", p->object_types_to_check.check_motorcycle); + updateParam(parameters, ns + "pedestrian", p->object_types_to_check.check_pedestrian); + } + + { + const std::string ns = "lane_change.regulation."; + updateParam(parameters, ns + "crosswalk", p->regulate_on_crosswalk); + updateParam(parameters, ns + "intersection", p->regulate_on_intersection); + updateParam(parameters, ns + "traffic_light", p->regulate_on_traffic_light); + } + + { + const std::string ns = "lane_change.stuck_detection."; + updateParam(parameters, ns + "velocity", p->stop_velocity_threshold); + updateParam(parameters, ns + "stop_time", p->stop_time_threshold); + } + + { + const std::string ns = "lane_change.cancel."; + bool enable_on_prepare_phase = true; + updateParam(parameters, ns + "enable_on_prepare_phase", enable_on_prepare_phase); + if (!enable_on_prepare_phase) { + RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change cancel function is disabled."); + p->cancel.enable_on_prepare_phase = enable_on_prepare_phase; + } + + bool enable_on_lane_changing_phase = true; + updateParam( + parameters, ns + "enable_on_lane_changing_phase", enable_on_lane_changing_phase); + if (!enable_on_lane_changing_phase) { + RCLCPP_WARN_ONCE(node_->get_logger(), "WARNING! Lane Change abort function is disabled."); + p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase; + } + + updateParam(parameters, ns + "delta_time", p->cancel.delta_time); + updateParam(parameters, ns + "duration", p->cancel.duration); + updateParam(parameters, ns + "max_lateral_jerk", p->cancel.max_lateral_jerk); + updateParam(parameters, ns + "overhang_tolerance", p->cancel.overhang_tolerance); + updateParam( + parameters, ns + "unsafe_hysteresis_threshold", p->cancel.unsafe_hysteresis_threshold); + } + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); + }); +} + +} // namespace autoware::behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_path_planner::LaneChangeRightModuleManager, + autoware::behavior_path_planner::SceneModuleManagerInterface) +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_path_planner::LaneChangeLeftModuleManager, + autoware::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/autoware_behavior_path_lane_change_module/src/scene.cpp new file mode 100644 index 0000000000000..44132f490c018 --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -0,0 +1,2252 @@ +// 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 "autoware_behavior_path_lane_change_module/scene.hpp" + +#include "autoware_behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using motion_utils::calcSignedArcLength; +using utils::lane_change::calcMinimumLaneChangeLength; +using utils::lane_change::createLanesPolygon; +using utils::path_safety_checker::isPolygonOverlapLanelet; +using utils::traffic_light::getDistanceToNextTrafficLight; + +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 + status_.current_lanes = status_.lane_change_path.info.current_lanes; + status_.target_lanes = status_.lane_change_path.info.target_lanes; + status_.is_valid_path = found_valid_path; + status_.is_safe = found_safe_path; + status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); + status_.lane_change_lane_ids = utils::getIds(status_.target_lanes); + + const auto arclength_start = + lanelet::utils::getArcCoordinates(status_.target_lanes, getEgoPose()); + status_.start_distance = arclength_start.length; + status_.lane_change_path.path.header = getRouteHeader(); +} + +std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) const +{ + const auto current_lanes = getCurrentLanes(); + + if (current_lanes.empty()) { + return {false, false}; + } + + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + + if (target_lanes.empty()) { + return {false, false}; + } + + // find candidate paths + LaneChangePaths 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); + } + + lane_change_debug_.valid_paths = valid_paths; + + if (valid_paths.empty()) { + safe_path.info.current_lanes = current_lanes; + safe_path.info.target_lanes = target_lanes; + return {false, false}; + } + + if (found_safe_path) { + safe_path = valid_paths.back(); + } else { + // force candidate + safe_path = valid_paths.front(); + } + + return {true, found_safe_path}; +} + +bool NormalLaneChange::isLaneChangeRequired() +{ + status_.current_lanes = getCurrentLanes(); + + if (status_.current_lanes.empty()) { + return false; + } + + status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_); + + return !status_.target_lanes.empty(); +} + +bool NormalLaneChange::isStoppedAtRedTrafficLight() const +{ + return utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( + status_.current_lanes, status_.lane_change_path.path, planner_data_, + status_.lane_change_path.info.length.sum()); +} + +TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() +{ + const auto original_turn_signal_info = prev_module_output_.turn_signal_info; + + const auto & current_lanes = getLaneChangeStatus().current_lanes; + const auto is_valid = getLaneChangeStatus().is_valid_path; + const auto & lane_change_path = getLaneChangeStatus().lane_change_path; + const auto & lane_change_param = getLaneChangeParam(); + + if (getModuleType() != LaneChangeModuleType::NORMAL || current_lanes.empty() || !is_valid) { + return original_turn_signal_info; + } + + // check direction + TurnSignalInfo current_turn_signal_info; + const auto & current_pose = getEgoPose(); + const auto direction = getDirection(); + if (direction == Direction::LEFT) { + current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else if (direction == Direction::RIGHT) { + current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + + const auto & path = prev_module_output_.path; + if (path.points.empty()) { + current_turn_signal_info.desired_start_point = current_pose; + current_turn_signal_info.required_start_point = current_pose; + current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; + current_turn_signal_info.required_end_point = lane_change_path.info.lane_changing_end; + return current_turn_signal_info; + } + + const auto min_length_for_turn_signal_activation = + lane_change_param.min_length_for_turn_signal_activation; + const auto & route_handler = getRouteHandler(); + const auto & common_parameter = getCommonParam(); + const auto shift_intervals = + route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); + const double next_lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals); + const double nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; + const double nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; + const double base_to_front = common_parameter.base_link2front; + + const double buffer = + next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; + const double path_length = motion_utils::calcArcLength(path.points); + const size_t current_nearest_seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); + const auto start_pose = + motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0)); + if (dist_to_terminal - base_to_front < buffer && start_pose) { + // modify turn signal + current_turn_signal_info.desired_start_point = *start_pose; + current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; + current_turn_signal_info.required_start_point = current_turn_signal_info.desired_start_point; + current_turn_signal_info.required_end_point = current_turn_signal_info.desired_end_point; + + const auto & original_command = original_turn_signal_info.turn_signal.command; + if ( + original_command == TurnIndicatorsCommand::DISABLE || + original_command == TurnIndicatorsCommand::NO_COMMAND) { + return current_turn_signal_info; + } + + // check the priority of turn signals + return getTurnSignalDecider().overwrite_turn_signal( + path, current_pose, current_nearest_seg_idx, original_turn_signal_info, + current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); + } + + // not in the vicinity of the end of the path. return original + return original_turn_signal_info; +} + +LaneChangePath NormalLaneChange::getLaneChangePath() const +{ + return status_.lane_change_path; +} + +BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const +{ + auto output = prev_module_output_; + + if (isAbortState() && abort_path_) { + output.path = abort_path_->path; + extendOutputDrivableArea(output); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, + output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + return output; + } + + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_DEBUG(logger_, "Current lanes not found. Returning previous module's path as output."); + return prev_module_output_; + } + + const auto terminal_path = + calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + if (!terminal_path) { + RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output."); + return prev_module_output_; + } + + output.path = terminal_path->path; + output.turn_signal_info = updateOutputTurnSignal(); + + extendOutputDrivableArea(output); + + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, + output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + return output; +} + +BehaviorModuleOutput NormalLaneChange::generateOutput() +{ + if (!status_.is_valid_path) { + RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output."); + return prev_module_output_; + } + + auto output = prev_module_output_; + if (isAbortState() && abort_path_) { + output.path = abort_path_->path; + insertStopPoint(status_.current_lanes, output.path); + } else { + output.path = getLaneChangePath().path; + + const auto found_extended_path = extendPath(); + if (found_extended_path) { + output.path = utils::combinePath(output.path, *found_extended_path); + } + output.reference_path = getReferencePath(); + output.turn_signal_info = updateOutputTurnSignal(); + + if (isStopState()) { + const auto current_velocity = getEgoVelocity(); + 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); + } else { + insertStopPoint(status_.target_lanes, output.path); + } + } + + extendOutputDrivableArea(output); + + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, + output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + return output; +} + +void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const +{ + const auto & dp = planner_data_->drivable_area_expansion_parameters; + + const auto drivable_lanes = utils::lane_change::generateDrivableLanes( + *getRouteHandler(), status_.current_lanes, status_.target_lanes); + const auto shorten_lanes = utils::cutOverlappedLanes(output.path, drivable_lanes); + const auto expanded_lanes = utils::expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + + // for new architecture + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = expanded_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, prev_module_output_.drivable_area_info); +} + +void NormalLaneChange::insertStopPoint( + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) +{ + if (lanelets.empty()) { + return; + } + + const auto & route_handler = getRouteHandler(); + + if (route_handler->getNumLaneToPreferredLane(lanelets.back()) == 0) { + return; + } + + const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); + const auto lane_change_buffer = + calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); + + 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 = getDistanceAlongLanelet(goal); + } else { + distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); + } + + const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; + const auto target_objects = filterObjects(status_.current_lanes, status_.target_lanes); + double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + + const auto is_valid_start_point = std::invoke([&]() -> bool { + auto lc_start_point = lanelet::utils::conversion::toLaneletPoint( + status_.lane_change_path.info.lane_changing_start.position); + const auto target_neighbor_preferred_lane_poly_2d = + utils::lane_change::getTargetNeighborLanesPolygon( + *route_handler, status_.current_lanes, type_); + return boost::geometry::covered_by( + lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d); + }); + + if (!is_valid_start_point) { + const auto stop_point = utils::insertStopPoint(stopping_distance, path); + setStopPose(stop_point.point.pose); + + return; + } + + // 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, lane_change_parameters_->minimum_lane_changing_velocity, + lane_change_parameters_->rss_params); + const double lane_change_buffer_for_blocking_object = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); + + const auto stopping_distance_for_obj = + distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - + lane_change_parameters_->backward_length_buffer_for_blocking_object - 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); + } +} + +PathWithLaneId NormalLaneChange::getReferencePath() const +{ + return utils::getCenterLinePathFromLanelet( + status_.lane_change_path.info.target_lanes.front(), planner_data_); +} + +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 = calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); + + if (dist < 0.0) { + return std::nullopt; + } + + auto & target_lanes = status_.target_lanes; + const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + const auto dist_in_target = lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); + + const auto forward_path_length = getCommonParam().forward_path_length; + + if ((target_lane_length - dist_in_target.length) > forward_path_length) { + return std::nullopt; + } + + const auto is_goal_in_target = getRouteHandler()->isInGoalRouteSection(target_lanes.back()); + + if (is_goal_in_target) { + const auto goal_pose = getRouteHandler()->getGoalPose(); + + const auto dist_to_goal = lanelet::utils::getArcCoordinates(target_lanes, goal_pose).length; + const auto dist_to_end_of_path = + lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; + + return getRouteHandler()->getCenterLinePath(target_lanes, dist_to_end_of_path, dist_to_goal); + } + + lanelet::ConstLanelet next_lane; + if (!getRouteHandler()->getNextLaneletWithinRoute(target_lanes.back(), &next_lane)) { + return std::nullopt; + } + + target_lanes.push_back(next_lane); + + const auto target_pose = std::invoke([&]() { + const auto is_goal_in_next_lane = getRouteHandler()->isInGoalRouteSection(next_lane); + if (is_goal_in_next_lane) { + return getRouteHandler()->getGoalPose(); + } + + Pose back_pose; + const auto back_point = + lanelet::utils::conversion::toGeomMsgPt(next_lane.centerline2d().back()); + const double front_yaw = lanelet::utils::getLaneletAngle(next_lane, back_point); + back_pose.position = back_point; + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, front_yaw); + back_pose.orientation = tf2::toMsg(tf_quat); + return back_pose; + }); + + const auto dist_to_target_pose = + lanelet::utils::getArcCoordinates(target_lanes, target_pose).length; + const auto dist_to_end_of_path = + lanelet::utils::getArcCoordinates(target_lanes, path.points.back().point.pose).length; + + return getRouteHandler()->getCenterLinePath( + target_lanes, dist_to_end_of_path, dist_to_target_pose); +} +void NormalLaneChange::resetParameters() +{ + is_abort_path_approved_ = false; + is_abort_approval_requested_ = false; + current_lane_change_state_ = LaneChangeStates::Normal; + abort_path_ = nullptr; + status_ = {}; + unsafe_hysteresis_count_ = 0; + lane_change_debug_.reset(); + + RCLCPP_DEBUG(logger_, "reset all flags and debug information."); +} + +TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const +{ + const auto & pose = getEgoPose(); + const auto & current_lanes = status_.current_lanes; + const auto & shift_line = status_.lane_change_path.info.shift_line; + const auto & shift_path = status_.lane_change_path.shifted_path; + const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + constexpr bool is_driving_forward = true; + // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's + // current lane, lane change is different, so we set this flag to false. + constexpr bool egos_lane_is_shifted = false; + + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward, + egos_lane_is_shifted); + return new_signal; +} + +lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const +{ + return utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); +} + +lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( + const lanelet::ConstLanelets & current_lanes, Direction direction) const +{ + if (current_lanes.empty()) { + return {}; + } + // Get lane change lanes + const auto & route_handler = getRouteHandler(); + + const auto lane_change_lane = utils::lane_change::getLaneChangeTargetLane( + *getRouteHandler(), current_lanes, type_, direction); + + if (!lane_change_lane) { + return {}; + } + + const auto front_pose = std::invoke([&lane_change_lane]() { + const auto & p = lane_change_lane->centerline().front(); + const auto front_point = lanelet::utils::conversion::toGeomMsgPt(p); + const auto front_yaw = lanelet::utils::getLaneletAngle(*lane_change_lane, front_point); + geometry_msgs::msg::Pose front_pose; + front_pose.position = front_point; + tf2::Quaternion quat; + quat.setRPY(0, 0, front_yaw); + front_pose.orientation = tf2::toMsg(quat); + return front_pose; + }); + + const auto forward_length = std::invoke([&]() { + const auto signed_distance = utils::getSignedDistance(front_pose, getEgoPose(), current_lanes); + const auto forward_path_length = planner_data_->parameters.forward_path_length; + if (signed_distance <= 0.0) { + return forward_path_length; + } + + return signed_distance + forward_path_length; + }); + const auto backward_length = lane_change_parameters_->backward_lane_length; + + return route_handler->getLaneletSequence( + lane_change_lane.value(), getEgoPose(), backward_length, forward_length); +} + +bool NormalLaneChange::isNearEndOfCurrentLanes( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const double threshold) const +{ + if (current_lanes.empty()) { + return false; + } + + const auto & route_handler = getRouteHandler(); + const auto & current_pose = getEgoPose(); + const auto lane_change_buffer = calcMinimumLaneChangeLength( + route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); + + const auto distance_to_lane_change_end = std::invoke([&]() { + auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); + + if (!target_lanes.empty() && route_handler->isInGoalRouteSection(target_lanes.back())) { + distance_to_end = std::min( + distance_to_end, + utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)); + } + + return std::max(0.0, distance_to_end) - lane_change_buffer; + }); + + lane_change_debug_.distance_to_end_of_current_lane = distance_to_lane_change_end; + return distance_to_lane_change_end < threshold; +} + +bool NormalLaneChange::hasFinishedLaneChange() const +{ + const auto & current_pose = getEgoPose(); + const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; + const double dist_to_lane_change_end = utils::getSignedDistance( + current_pose, lane_change_end, status_.lane_change_path.info.target_lanes); + double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; + + // If ego velocity is low, relax finish judge buffer + const double ego_velocity = getEgoVelocity(); + if (std::abs(ego_velocity) < 1.0) { + finish_judge_buffer = 0.0; + } + + const auto reach_lane_change_end = dist_to_lane_change_end + finish_judge_buffer < 0.0; + + lane_change_debug_.distance_to_lane_change_finished = + dist_to_lane_change_end + finish_judge_buffer; + + if (!reach_lane_change_end) { + return false; + } + + const auto arc_length = lanelet::utils::getArcCoordinates(status_.target_lanes, current_pose); + const auto reach_target_lane = + std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; + + lane_change_debug_.distance_to_lane_change_finished = arc_length.distance; + + return reach_target_lane; +} + +bool NormalLaneChange::isAbleToReturnCurrentLane() const +{ + if (status_.lane_change_path.path.points.size() < 2) { + lane_change_debug_.is_able_to_return_to_current_lane = false; + return false; + } + + if (!utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters, + lane_change_parameters_->cancel.overhang_tolerance)) { + lane_change_debug_.is_able_to_return_to_current_lane = false; + return false; + } + + const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + status_.lane_change_path.path.points, getEgoPose(), + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + const double ego_velocity = + std::max(getEgoVelocity(), lane_change_parameters_->minimum_lane_changing_velocity); + const double estimated_travel_dist = ego_velocity * lane_change_parameters_->cancel.delta_time; + + double dist = 0.0; + for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { + 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; + auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( + status_.current_lanes, estimated_pose, planner_data_->parameters, + lane_change_parameters_->cancel.overhang_tolerance); + lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; + return is_ego_within_original_lane; + } + } + + lane_change_debug_.is_able_to_return_to_current_lane = true; + return true; +} + +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 calcSignedArcLength(path_points, start_position, getEgoPosition()) < 0.0; +} + +bool NormalLaneChange::isAbleToStopSafely() const +{ + if (status_.lane_change_path.path.points.size() < 2) { + return false; + } + + const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + status_.lane_change_path.path.points, getEgoPose(), + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + const auto current_velocity = getEgoVelocity(); + const auto stop_dist = + -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + + double dist = 0.0; + for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { + 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( + status_.current_lanes, estimated_pose, planner_data_->parameters); + } + } + return true; +} + +bool NormalLaneChange::hasFinishedAbort() const +{ + if (!abort_path_) { + lane_change_debug_.is_abort = true; + return true; + } + + const auto distance_to_finish = calcSignedArcLength( + abort_path_->path.points, getEgoPosition(), abort_path_->info.shift_line.end.position); + lane_change_debug_.distance_to_abort_finished = distance_to_finish; + + const auto has_finished_abort = distance_to_finish < 0.0; + lane_change_debug_.is_abort = has_finished_abort; + + return has_finished_abort; +} + +bool NormalLaneChange::isAbortState() const +{ + if (!lane_change_parameters_->cancel.enable_on_lane_changing_phase) { + return false; + } + + if (current_lane_change_state_ != LaneChangeStates::Abort) { + return false; + } + + if (!abort_path_) { + return false; + } + + lane_change_debug_.is_abort = true; + return true; +} +int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const +{ + const auto get_opposite_direction = + (direction_ == Direction::RIGHT) ? Direction::LEFT : Direction::RIGHT; + 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_output_.path.points, getEgoPose(), p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold); + const auto max_path_velocity = + prev_module_output_.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, *lane_change_parameters_); + const auto max_acc = utils::lane_change::calcMaximumAcceleration( + getEgoVelocity(), max_path_velocity, vehicle_max_acc, *lane_change_parameters_); + + 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( + std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()), + *lane_change_parameters_, shift_intervals, max_acc); +} + +std::vector NormalLaneChange::sampleLongitudinalAccValues( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + if (prev_module_output_.path.points.empty()) { + return {}; + } + + const auto & route_handler = *getRouteHandler(); + const auto current_pose = getEgoPose(); + const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; + + 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 = 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); + } + + // if maximum lane change length is less than length to goal or the end of target lanes, only + // sample max acc + 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); +} + +std::vector NormalLaneChange::calcPrepareDuration( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + 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; + + for (double duration = lane_change_parameters_->lane_change_prepare_duration; duration >= 0.0; + duration -= step) { + prepare_durations.push_back(duration); + if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { + break; + } + } + + return prepare_durations; +} + +PathWithLaneId NormalLaneChange::getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double backward_path_length, + const double prepare_length) const +{ + if (current_lanes.empty()) { + return PathWithLaneId(); + } + + auto prepare_segment = prev_module_output_.path; + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prepare_segment.points, getEgoPose(), 3.0, 1.0); + utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); + + return prepare_segment; +} + +ExtendedPredictedObjects NormalLaneChange::getTargetObjects( + const LaneChangeLanesFilteredObjects & filtered_objects, + const lanelet::ConstLanelets & current_lanes) const +{ + ExtendedPredictedObjects target_objects = filtered_objects.target_lane; + const auto is_stuck = isVehicleStuck(current_lanes); + const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes; + if (chk_obj_in_curr_lanes || is_stuck) { + target_objects.insert( + target_objects.end(), filtered_objects.current_lane.begin(), + filtered_objects.current_lane.end()); + } + + const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes; + if (chk_obj_in_other_lanes) { + target_objects.insert( + target_objects.end(), filtered_objects.other_lane.begin(), filtered_objects.other_lane.end()); + } + + return target_objects; +} + +LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + const auto & current_pose = getEgoPose(); + const auto & route_handler = getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + auto objects = *planner_data_->dynamic_object; + utils::path_safety_checker::filterObjectsByClass( + objects, lane_change_parameters_->object_types_to_check); + + if (objects.objects.empty()) { + return {}; + } + + filterOncomingObjects(objects); + + if (objects.objects.empty()) { + return {}; + } + + filterAheadTerminalObjects(objects, current_lanes); + + if (objects.objects.empty()) { + return {}; + } + + std::vector target_lane_objects; + std::vector current_lane_objects; + std::vector other_lane_objects; + + filterObjectsByLanelets( + objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects, + other_lane_objects); + + const auto is_within_vel_th = [](const auto & object) -> bool { + constexpr double min_vel_th = 1.0; + constexpr double max_vel_th = std::numeric_limits::max(); + return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); + }; + + const auto path = + route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + + if (path.points.empty()) { + return {}; + } + + const auto is_ahead_of_ego = [&path, ¤t_pose](const auto & object) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object).outer(); + + double max_dist_ego_to_obj = std::numeric_limits::lowest(); + for (const auto & polygon_p : obj_polygon) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const auto 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); + } + return max_dist_ego_to_obj >= 0.0; + }; + + utils::path_safety_checker::filterObjects( + target_lane_objects, [&](const PredictedObject & object) { + return (is_within_vel_th(object) || is_ahead_of_ego(object)); + }); + + utils::path_safety_checker::filterObjects( + other_lane_objects, [&](const PredictedObject & object) { + return is_within_vel_th(object) && is_ahead_of_ego(object); + }); + + utils::path_safety_checker::filterObjects( + current_lane_objects, [&](const PredictedObject & object) { + return is_within_vel_th(object) && is_ahead_of_ego(object); + }); + + LaneChangeLanesFilteredObjects lane_change_target_objects; + + const auto is_check_prepare_phase = check_prepare_phase(); + std::for_each(target_lane_objects.begin(), target_lane_objects.end(), [&](const auto & object) { + auto extended_predicted_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); + lane_change_target_objects.target_lane.push_back(extended_predicted_object); + }); + + std::for_each(current_lane_objects.begin(), current_lane_objects.end(), [&](const auto & object) { + auto extended_predicted_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); + lane_change_target_objects.current_lane.push_back(extended_predicted_object); + }); + + std::for_each(other_lane_objects.begin(), other_lane_objects.end(), [&](const auto & object) { + auto extended_predicted_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, is_check_prepare_phase); + lane_change_target_objects.other_lane.push_back(extended_predicted_object); + }); + + lane_change_debug_.filtered_objects = lane_change_target_objects; + + return lane_change_target_objects; +} + +void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const +{ + const auto & current_pose = getEgoPose(); + + const auto is_same_direction = [&](const PredictedObject & object) { + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; + return !utils::path_safety_checker::isTargetObjectOncoming(current_pose, object_pose); + }; + + // Perception noise could make stationary objects seem opposite the ego vehicle; check the + // velocity to prevent this. + const auto is_stopped_object = [](const auto & object) -> bool { + constexpr double min_vel_th = -0.5; + constexpr double max_vel_th = 0.5; + return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th); + }; + + utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { + const auto same_direction = is_same_direction(object); + if (same_direction) { + return true; + } + + return is_stopped_object(object); + }); +} + +void NormalLaneChange::filterAheadTerminalObjects( + PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const +{ + const auto & current_pose = getEgoPose(); + const auto & route_handler = getRouteHandler(); + const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( + route_handler, current_lanes.back(), *lane_change_parameters_, direction_); + + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); + + // ignore object that are ahead of terminal lane change start + utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object).outer(); + // ignore object that are ahead of terminal lane change start + auto distance_to_terminal_from_object = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + Pose polygon_pose; + polygon_pose.position = obj_p; + polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; + const auto dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes); + distance_to_terminal_from_object = std::min(dist_ego_to_current_lanes_center, dist); + } + + return (minimum_lane_change_length > distance_to_terminal_from_object); + }); +} + +void NormalLaneChange::filterObjectsByLanelets( + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, std::vector & current_lane_objects, + std::vector & target_lane_objects, + std::vector & other_lane_objects) const +{ + const auto & current_pose = getEgoPose(); + const auto & route_handler = getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + const auto check_optional_polygon = [](const auto & object, const auto & polygon) { + return polygon && isPolygonOverlapLanelet(object, *polygon); + }; + + // get backward lanes + const auto backward_length = lane_change_parameters_->backward_lane_length; + const auto target_backward_lanes = + utils::getPrecedingLanelets(*route_handler, target_lanes, current_pose, backward_length); + + { + lane_change_debug_.current_lanes = current_lanes; + lane_change_debug_.target_lanes = target_lanes; + + // TODO(Azu) change the type to std::vector + lane_change_debug_.target_backward_lanes.clear(); + std::for_each( + target_backward_lanes.begin(), target_backward_lanes.end(), + [&](const lanelet::ConstLanelets & target_backward_lane) { + lane_change_debug_.target_backward_lanes.insert( + lane_change_debug_.target_backward_lanes.end(), target_backward_lane.begin(), + target_backward_lane.end()); + }); + } + + 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 lanes_polygon = + createLanesPolygon(current_lanes, expanded_target_lanes, target_backward_lanes); + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); + + { + const auto reserve_size = objects.objects.size(); + current_lane_objects.reserve(reserve_size); + target_lane_objects.reserve(reserve_size); + other_lane_objects.reserve(reserve_size); + } + + for (const auto & object : objects.objects) { + const auto is_lateral_far = std::invoke([&]() -> bool { + 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); + }); + + if (check_optional_polygon(object, lanes_polygon.target) && is_lateral_far) { + target_lane_objects.push_back(object); + continue; + } + + const auto is_overlap_target_backward = std::invoke([&]() -> bool { + const auto check_backward_polygon = [&object](const auto & target_backward_polygon) { + return isPolygonOverlapLanelet(object, target_backward_polygon); + }; + return std::any_of( + lanes_polygon.target_backward.begin(), lanes_polygon.target_backward.end(), + check_backward_polygon); + }); + + // check if the object intersects with target backward lanes + if (is_overlap_target_backward) { + target_lane_objects.push_back(object); + continue; + } + + if (check_optional_polygon(object, lanes_polygon.current)) { + // check only the objects that are in front of the ego vehicle + current_lane_objects.push_back(object); + continue; + } + + other_lane_objects.push_back(object); + } +} + +PathWithLaneId NormalLaneChange::getTargetSegment( + const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, + const double target_lane_length, const double lane_changing_length, + const double lane_changing_velocity, const double buffer_for_next_lane_change) const +{ + const auto & route_handler = *getRouteHandler(); + const auto forward_path_length = planner_data_->parameters.forward_path_length; + + const double s_start = std::invoke([&lane_changing_start_pose, &target_lanes, + &lane_changing_length, &target_lane_length, + &buffer_for_next_lane_change]() { + const auto arc_to_start_pose = + lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); + const double dist_from_front_target_lanelet = arc_to_start_pose.length + lane_changing_length; + const double end_of_lane_dist_without_buffer = target_lane_length - buffer_for_next_lane_change; + return std::min(dist_from_front_target_lanelet, end_of_lane_dist_without_buffer); + }); + + const double s_end = std::invoke( + [&s_start, &forward_path_length, &target_lane_length, &buffer_for_next_lane_change]() { + const double dist_from_start = s_start + forward_path_length; + const double dist_from_end = target_lane_length - buffer_for_next_lane_change; + return std::max( + std::min(dist_from_start, dist_from_end), s_start + std::numeric_limits::epsilon()); + }); + + RCLCPP_DEBUG(logger_, "in %s start: %f, end: %f", __func__, s_start, s_end); + + PathWithLaneId target_segment = route_handler.getCenterLinePath(target_lanes, s_start, s_end); + for (auto & point : target_segment.points) { + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_velocity)); + } + + return target_segment; +} + +bool NormalLaneChange::hasEnoughLength( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const Direction direction) const +{ + if (target_lanes.empty()) { + return false; + } + + const auto current_pose = getEgoPose(); + const auto & route_handler = getRouteHandler(); + const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); + const auto minimum_lane_change_length_to_preferred_lane = calcMinimumLaneChangeLength( + route_handler, target_lanes.back(), *lane_change_parameters_, direction); + + const double lane_change_length = path.info.length.sum(); + if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + return false; + } + + const auto goal_pose = route_handler->getGoalPose(); + if ( + route_handler->isInGoalRouteSection(current_lanes.back()) && + lane_change_length + minimum_lane_change_length_to_preferred_lane > + utils::getSignedDistance(current_pose, goal_pose, current_lanes)) { + return false; + } + + // return if there are no target lanes + if ( + lane_change_length + minimum_lane_change_length_to_preferred_lane > + utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + return false; + } + + 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::hasEnoughLengthToTrafficLight( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto dist_to_next_traffic_light = + getDistanceToNextTrafficLight(current_pose, current_lanes); + const auto dist_to_next_traffic_light_from_lc_start_pose = + dist_to_next_traffic_light - path.info.length.prepare; + + return dist_to_next_traffic_light_from_lc_start_pose <= 0.0 || + dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing; +} + +bool NormalLaneChange::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) const +{ + lane_change_debug_.collision_check_objects.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(); + const auto & common_parameters = planner_data_->parameters; + + const auto backward_path_length = common_parameters.backward_path_length; + const auto forward_path_length = common_parameters.forward_path_length; + const auto minimum_lane_changing_velocity = + lane_change_parameters_->minimum_lane_changing_velocity; + const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; + + // get velocity + const auto current_velocity = getEgoVelocity(); + + // get sampling acceleration values + const auto longitudinal_acc_sampling_values = + sampleLongitudinalAccValues(current_lanes, target_lanes); + + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + + const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + + const auto dist_to_end_of_current_lanes = + utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); + + const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + + const auto sorted_lane_ids = + utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + + 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 filtered_objects = filterObjects(current_lanes, target_lanes); + const auto target_objects = getTargetObjects(filtered_objects, current_lanes); + + const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); + + candidate_paths->reserve( + longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); + + 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::clamp( + current_velocity + sampled_longitudinal_acc * prepare_duration, + minimum_lane_changing_velocity, getCommonParam().max_vel); + + // compute actual longitudinal acceleration + const double longitudinal_acc_on_prepare = + (prepare_duration < 1e-3) ? 0.0 + : ((prepare_velocity - current_velocity) / prepare_duration); + + const auto prepare_length = utils::lane_change::calcPhaseLength( + current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration); + + auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + + if (prepare_segment.points.empty()) { + debug_print("prepare segment is empty...? Unexpected."); + continue; + } + + // 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); + + // 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; + } + + 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] = + lane_change_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; + + 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); + } + 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, lane_change_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 = utils::lane_change::calcPhaseLength( + initial_lane_changing_velocity, getCommonParam().max_vel, + longitudinal_acc_on_lane_changing, lane_changing_time); + const auto terminal_lane_changing_velocity = std::min( + initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, + getCommonParam().max_vel); + 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; + } + + 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 : lane_change_parameters_->backward_length_buffer_for_end_of_lane; + const double finish_judge_buffer = + lane_change_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; + } + } + + 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 (target_segment.points.empty()) { + debug_print("Reject: target segment is empty!! something wrong..."); + continue; + } + + const lanelet::BasicPoint2d lc_start_point( + prepare_segment.points.back().point.pose.position.x, + prepare_segment.points.back().point.pose.position.y); + + 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 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); + + 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.lateral_acceleration = lateral_acc; + lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; + + if (!is_valid_start_point) { + debug_print( + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); + 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); + + if (target_lane_reference_path.points.empty()) { + debug_print("Reject: target_lane_reference_path is empty!!"); + continue; + } + + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + prepare_segment, target_segment, target_lane_reference_path, shift_length); + + 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 (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { + debug_print("Reject: invalid candidate path!!"); + continue; + } + + 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."); + } + + if ( + lane_change_parameters_->regulate_on_traffic_light && + !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) { + debug_print("Reject: regulate on traffic light!!"); + continue; + } + + if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( + lane_change_info.current_lanes, candidate_path.value().path, planner_data_, + lane_change_info.length.sum())) { + debug_print("Ego is stopping near traffic light. Do not allow lane change"); + continue; + } + candidate_paths->push_back(*candidate_path); + + if ( + !is_stuck && utils::lane_change::passParkedObject( + route_handler, *candidate_path, filtered_objects.target_lane, + lane_change_buffer, is_goal_in_route, *lane_change_parameters_, + lane_change_debug_.collision_check_objects)) { + 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) { + 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, rss_params, lane_change_debug_.collision_check_objects); + + 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; +} + +std::optional NormalLaneChange::calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + if (current_lanes.empty() || target_lanes.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + const auto & route_handler = *getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + + const auto forward_path_length = common_parameters.forward_path_length; + const auto minimum_lane_changing_velocity = + lane_change_parameters_->minimum_lane_changing_velocity; + + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + + const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + + const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + + const auto sorted_lane_ids = + utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + + 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 {}; + } + + // lane changing start getEgoPose() is at the end of prepare segment + const auto current_lane_terminal_point = + lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); + + double distance_to_terminal_from_goal = 0; + if (is_goal_in_route) { + distance_to_terminal_from_goal = + utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); + } + + const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( + prev_module_output_.path.points, current_lane_terminal_point, + -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); + + if (!lane_changing_start_pose) { + RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); + return {}; + } + + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose.value()); + + // 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_, "lane change start getEgoPose() is behind target lanelet!"); + return {}; + } + + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( + target_lanes, lane_changing_start_pose.value()); + + const auto [min_lateral_acc, max_lateral_acc] = + lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); + + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + + const auto target_segment = getTargetSegment( + target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer, + minimum_lane_changing_velocity, next_lane_change_buffer); + + if (target_segment.points.empty()) { + RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); + return {}; + } + + const lanelet::BasicPoint2d lc_start_point( + lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); + + 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 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); + + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; + lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = lane_changing_start_pose.value(); + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.lateral_acceleration = max_lateral_acc; + lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; + + if (!is_valid_start_point) { + RCLCPP_DEBUG( + logger_, + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); + return {}; + } + + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lane_change_buffer, minimum_lane_changing_velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, + lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route, + next_lane_change_buffer); + + if (target_lane_reference_path.points.empty()) { + RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); + return {}; + } + + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + lane_changing_start_pose.value(), target_segment.points.front().point.pose, + target_lane_reference_path, shift_length); + + auto reference_segment = prev_module_output_.path; + const double length_to_lane_changing_start = motion_utils::calcSignedArcLength( + reference_segment.points, reference_segment.points.front().point.pose.position, + lane_changing_start_pose->position); + utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); + // remove terminal points because utils::clipPathLength() calculates extra long path + reference_segment.points.pop_back(); + reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; + + const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( + lane_change_info, reference_segment, target_segment, target_lane_reference_path, + sorted_lane_ids); + + return terminal_lane_change_path; +} + +PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const +{ + const auto & path = status_.lane_change_path; + const auto & current_lanes = status_.current_lanes; + const auto & target_lanes = status_.target_lanes; + + const auto filtered_objects = filterObjects(current_lanes, target_lanes); + const auto target_objects = getTargetObjects(filtered_objects, current_lanes); + + CollisionCheckDebugMap debug_data; + const auto safety_status = isLaneChangePathSafe( + path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); + { + // only for debug purpose + lane_change_debug_.collision_check_objects.clear(); + lane_change_debug_.collision_check_object_debug_lifetime += + (stop_watch_.toc(getModuleTypeStr()) / 1000); + if (lane_change_debug_.collision_check_object_debug_lifetime > 2.0) { + stop_watch_.toc(getModuleTypeStr(), true); + lane_change_debug_.collision_check_object_debug_lifetime = 0.0; + lane_change_debug_.collision_check_objects_after_approval.clear(); + } + + if (!safety_status.is_safe) { + lane_change_debug_.collision_check_objects_after_approval = debug_data; + } + } + + return safety_status; +} + +PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( + PathSafetyStatus approved_path_safety_status) +{ + if (!approved_path_safety_status.is_safe) { + ++unsafe_hysteresis_count_; + RCLCPP_DEBUG( + logger_, "%s: Increasing hysteresis count to %d.", __func__, unsafe_hysteresis_count_); + } else { + if (unsafe_hysteresis_count_ > 0) { + RCLCPP_DEBUG(logger_, "%s: Lane change is now SAFE. Resetting hysteresis count.", __func__); + } + unsafe_hysteresis_count_ = 0; + } + if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.unsafe_hysteresis_threshold) { + RCLCPP_DEBUG( + logger_, "%s: hysteresis count exceed threshold. lane change is now %s", __func__, + (approved_path_safety_status.is_safe ? "safe" : "UNSAFE")); + return approved_path_safety_status; + } + return {}; +} + +bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const +{ + const auto & route_handler = planner_data_->route_handler; + const auto & dp = planner_data_->drivable_area_expansion_parameters; + + // check lane departure + const auto drivable_lanes = utils::lane_change::generateDrivableLanes( + *route_handler, utils::extendLanes(route_handler, status_.current_lanes), + utils::extendLanes(route_handler, status_.target_lanes)); + const auto expanded_lanes = utils::expandLanelets( + drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + + const auto lanelets = utils::transformToLanelets(expanded_lanes); + + // check path points are in any lanelets + for (const auto & point : path.points) { + bool is_in_lanelet = false; + for (const auto & lanelet : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) { + is_in_lanelet = true; + break; + } + } + if (!is_in_lanelet) { + return false; + } + } + + // check relative angle + if (!utils::checkPathRelativeAngle(path, M_PI)) { + return false; + } + + return true; +} + +bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) +{ + const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; + if ( + isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && + isAbleToStopSafely() && is_object_coming_from_rear) { + current_lane_change_state_ = LaneChangeStates::Stop; + return true; + } + current_lane_change_state_ = LaneChangeStates::Normal; + return false; +} + +bool NormalLaneChange::calcAbortPath() +{ + const auto & route_handler = getRouteHandler(); + const auto & common_param = getCommonParam(); + const auto current_velocity = + std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); + const auto current_pose = getEgoPose(); + const auto & selected_path = status_.lane_change_path; + const auto reference_lanelets = selected_path.info.current_lanes; + + const auto ego_nearest_dist_threshold = common_param.ego_nearest_dist_threshold; + const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; + + const auto direction = getDirection(); + const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( + route_handler, selected_path.info.current_lanes.back(), *lane_change_parameters_, direction); + + const auto & lane_changing_path = selected_path.path; + const auto lane_changing_end_pose_idx = std::invoke([&]() { + constexpr double s_start = 0.0; + const double s_end = std::max( + lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, 0.0); + + const auto ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end); + return motion_utils::findFirstNearestIndexWithSoftConstraints( + lane_changing_path.points, ref.points.back().point.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + }); + + const auto ego_pose_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + lane_changing_path.points, current_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + + const auto get_abort_idx_and_distance = [&](const double param_time) { + if (ego_pose_idx > lane_changing_end_pose_idx) { + return std::make_pair(ego_pose_idx, 0.0); + } + + const auto desired_distance = current_velocity * param_time; + const auto & points = lane_changing_path.points; + + for (size_t idx = ego_pose_idx; idx < lane_changing_end_pose_idx; ++idx) { + const double distance = + utils::getSignedDistance(current_pose, points.at(idx).point.pose, reference_lanelets); + if (distance > desired_distance) { + return std::make_pair(idx, distance); + } + } + + return std::make_pair(ego_pose_idx, 0.0); + }; + + const auto [abort_start_idx, abort_start_dist] = + get_abort_idx_and_distance(lane_change_parameters_->cancel.delta_time); + const auto [abort_return_idx, abort_return_dist] = get_abort_idx_and_distance( + lane_change_parameters_->cancel.delta_time + lane_change_parameters_->cancel.duration); + + if (abort_start_idx >= abort_return_idx) { + RCLCPP_ERROR(logger_, "abort start idx and return idx is equal. can't compute abort path."); + return false; + } + + if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( + route_handler, reference_lanelets, current_pose, abort_return_dist, + *lane_change_parameters_, direction)) { + RCLCPP_ERROR(logger_, "insufficient distance to abort."); + return false; + } + + const auto abort_start_pose = lane_changing_path.points.at(abort_start_idx).point.pose; + const auto abort_return_pose = lane_changing_path.points.at(abort_return_idx).point.pose; + const auto shift_length = + lanelet::utils::getArcCoordinates(reference_lanelets, abort_return_pose).distance; + + ShiftLine shift_line; + shift_line.start = abort_start_pose; + shift_line.end = abort_return_pose; + shift_line.end_shift_length = -shift_length; + shift_line.start_idx = abort_start_idx; + shift_line.end_idx = abort_return_idx; + + PathShifter path_shifter; + path_shifter.setPath(lane_changing_path); + path_shifter.addShiftLine(shift_line); + const auto lateral_jerk = + autoware::behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( + shift_line.end_shift_length, abort_start_dist, current_velocity); + path_shifter.setVelocity(current_velocity); + const auto lateral_acc_range = + lane_change_parameters_->lane_change_lat_acc_map.find(current_velocity); + const double & max_lateral_acc = lateral_acc_range.second; + path_shifter.setLateralAccelerationLimit(max_lateral_acc); + + if (lateral_jerk > lane_change_parameters_->cancel.max_lateral_jerk) { + RCLCPP_ERROR(logger_, "Aborting jerk is too strong. lateral_jerk = %f", lateral_jerk); + return false; + } + + ShiftedPath shifted_path; + // offset front side + if (!path_shifter.generate(&shifted_path)) { + RCLCPP_ERROR(logger_, "failed to generate abort shifted path."); + } + + auto reference_lane_segment = prev_module_output_.path; + { + // const auto terminal_path = + // calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + // if (terminal_path) { + // reference_lane_segment = terminal_path->path; + // } + const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose; + const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold, + common_param.ego_nearest_yaw_threshold); + reference_lane_segment.points = motion_utils::cropPoints( + reference_lane_segment.points, return_pose.position, seg_idx, + common_param.forward_path_length, 0.0); + } + + auto abort_path = selected_path; + abort_path.shifted_path = shifted_path; + abort_path.info.shift_line = shift_line; + + { + PathWithLaneId aborting_path; + aborting_path.points.insert( + aborting_path.points.begin(), shifted_path.path.points.begin(), + shifted_path.path.points.begin() + abort_return_idx); + + if (!reference_lane_segment.points.empty()) { + abort_path.path = utils::combinePath(aborting_path, reference_lane_segment); + } else { + abort_path.path = aborting_path; + } + } + + abort_path_ = std::make_shared(abort_path); + return true; +} + +PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( + const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & collision_check_objects, + const utils::path_safety_checker::RSSparams & rss_params, + CollisionCheckDebugMap & debug_data) const +{ + PathSafetyStatus path_safety_status; + + if (collision_check_objects.empty()) { + RCLCPP_DEBUG(logger_, "There is nothing to check."); + return path_safety_status; + } + + const auto & path = lane_change_path.path; + const auto & common_parameters = planner_data_->parameters; + const auto current_pose = getEgoPose(); + const auto current_twist = getEgoTwist(); + + if (path.points.empty()) { + path_safety_status.is_safe = false; + return path_safety_status; + } + + const double & time_resolution = lane_change_parameters_->prediction_time_resolution; + + const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( + lane_change_path, current_twist, current_pose, common_parameters, *lane_change_parameters_, + time_resolution); + const auto debug_predicted_path = + utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); + + const auto current_lanes = getCurrentLanes(); + + 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 = utils::path_safety_checker::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) { + const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + get_max_velocity_for_safety_check(), current_debug_data.second); + + if (collided_polygons.empty()) { + utils::path_safety_checker::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) { + utils::path_safety_checker::updateCollisionCheckDebugMap( + debug_data, current_debug_data, is_safe); + continue; + } + + is_safe = false; + path_safety_status.is_safe = false; + utils::path_safety_checker::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); + } + utils::path_safety_checker::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 : lane_change_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 lane_change_buffer = calcMinimumLaneChangeLength( + route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); + const double stop_point_buffer = lane_change_parameters_->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; +} + +double NormalLaneChange::get_max_velocity_for_safety_check() const +{ + const auto external_velocity_limit_ptr = planner_data_->external_limit_max_velocity; + if (external_velocity_limit_ptr) { + return std::min( + static_cast(external_velocity_limit_ptr->max_velocity), getCommonParam().max_vel); + } + + return getCommonParam().max_vel; +} + +bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const +{ + if (current_lanes.empty()) { + lane_change_debug_.is_stuck = false; + 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, lane_change_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); + + auto is_vehicle_stuck = isVehicleStuck(current_lanes, detection_distance); + + lane_change_debug_.is_stuck = is_vehicle_stuck; + return is_vehicle_stuck; +} + +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"); +} + +bool NormalLaneChange::check_prepare_phase() const +{ + const auto & route_handler = getRouteHandler(); + const auto & vehicle_info = getCommonParam().vehicle_info; + + const auto check_in_general_lanes = + lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes; + + lanelet::ConstLanelet current_lane; + if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_DEBUG( + logger_, "Unable to get current lane. Default to %s.", + (check_in_general_lanes ? "true" : "false")); + return check_in_general_lanes; + } + + const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info); + + const auto check_in_intersection = std::invoke([&]() { + if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection) { + return false; + } + + return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint); + }); + + const auto check_in_turns = std::invoke([&]() { + if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns) { + return false; + } + + return utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint); + }); + + return check_in_intersection || check_in_turns || check_in_general_lanes; +} +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/autoware_behavior_path_lane_change_module/src/utils/markers.cpp similarity index 96% rename from planning/behavior_path_lane_change_module/src/utils/markers.cpp rename to planning/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index 708c561bfb629..2306eef90e61d 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/marker_utils/colors.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/colors.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include diff --git a/planning/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/autoware_behavior_path_lane_change_module/src/utils/utils.cpp new file mode 100644 index 0000000000000..b07d2100f210a --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -0,0 +1,1269 @@ +// Copyright 2021 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 "autoware_behavior_path_lane_change_module/utils/utils.hpp" + +#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware_behavior_path_lane_change_module/utils/path.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" +#include "tier4_autoware_utils/math/unit_conversion.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::behavior_path_planner::utils::lane_change +{ +using autoware::route_handler::RouteHandler; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Pose; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; + +using lanelet::ArcCoordinates; +using tier4_planning_msgs::msg::PathPointWithLaneId; + +rclcpp::Logger get_logger() +{ + constexpr const char * name{"lane_change.utils"}; + return rclcpp::get_logger(name); +} + +double calcLaneChangeResampleInterval( + const double lane_changing_length, const double lane_changing_velocity) +{ + constexpr auto min_resampling_points{30.0}; + constexpr auto resampling_dt{0.2}; + return std::max( + lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); +} + +double calcMinimumLaneChangeLength( + const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals) +{ + if (shift_intervals.empty()) { + return 0.0; + } + + const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity; + const auto [min_lat_acc, max_lat_acc] = + lane_change_parameters.lane_change_lat_acc_map.find(min_vel); + const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + + const auto calc_sum = [&](double sum, double shift_interval) { + const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + return sum + (min_vel * t + finish_judge_buffer); + }; + + const auto total_length = + std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); + + const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; + return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); +} + +double calcMinimumLaneChangeLength( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, + const LaneChangeParameters & lane_change_parameters, Direction direction) +{ + if (!route_handler) { + return std::numeric_limits::max(); + } + + const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lane, direction); + return utils::lane_change::calcMinimumLaneChangeLength(lane_change_parameters, shift_intervals); +} + +double calcMaximumLaneChangeLength( + const double current_velocity, const LaneChangeParameters & lane_change_parameters, + const std::vector & shift_intervals, const double max_acc) +{ + if (shift_intervals.empty()) { + return 0.0; + } + + const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const auto t_prepare = lane_change_parameters.lane_change_prepare_duration; + + auto vel = current_velocity; + + const auto calc_sum = [&](double sum, double shift_interval) { + // prepare section + const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; + vel = vel + max_acc * t_prepare; + + // lane changing section + const auto [min_lat_acc, max_lat_acc] = + lane_change_parameters.lane_change_lat_acc_map.find(vel); + const auto t_lane_changing = + PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + const auto lane_changing_length = + vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; + + vel = vel + max_acc * t_lane_changing; + return sum + (prepare_length + lane_changing_length + finish_judge_buffer); + }; + + const auto total_length = + std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); + + const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; + return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); +} + +double calcMinimumAcceleration( + const double current_velocity, const double min_longitudinal_acc, + const LaneChangeParameters & lane_change_parameters) +{ + const double min_lane_changing_velocity = lane_change_parameters.minimum_lane_changing_velocity; + const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; + const double acc = (min_lane_changing_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); +} + +double calcMaximumAcceleration( + const double current_velocity, const double current_max_velocity, + const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters) +{ + const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; + const double acc = (current_max_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, 0.0, max_longitudinal_acc); +} + +double calcLaneChangingAcceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc) +{ + if (prepare_longitudinal_acc <= 0.0) { + return 0.0; + } + + return std::clamp( + (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, + prepare_longitudinal_acc); +} + +void setPrepareVelocity( + PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) +{ + if (current_velocity < prepare_velocity) { + // acceleration + for (size_t i = 0; i < prepare_segment.points.size(); ++i) { + prepare_segment.points.at(i).point.longitudinal_velocity_mps = std::min( + prepare_segment.points.at(i).point.longitudinal_velocity_mps, + static_cast(prepare_velocity)); + } + } else { + // deceleration + prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( + prepare_segment.points.back().point.longitudinal_velocity_mps, + static_cast(prepare_velocity)); + } +} + +std::vector getAccelerationValues( + const double min_acc, const double max_acc, const size_t sampling_num) +{ + if (min_acc > max_acc) { + return {}; + } + + if (max_acc - min_acc < std::numeric_limits::epsilon()) { + return {0.0}; + } + + constexpr double epsilon = 0.001; + const auto resolution = std::abs(max_acc - min_acc) / sampling_num; + + std::vector sampled_values{min_acc}; + for (double sampled_acc = min_acc + resolution; + sampled_acc < max_acc + std::numeric_limits::epsilon(); sampled_acc += resolution) { + // check whether if we need to add 0.0 + if (sampled_values.back() < -epsilon && sampled_acc > epsilon) { + sampled_values.push_back(0.0); + } + + sampled_values.push_back(sampled_acc); + } + std::reverse(sampled_values.begin(), sampled_values.end()); + + return sampled_values; +} + +lanelet::ConstLanelets getTargetPreferredLanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const Direction & direction, + const LaneChangeModuleType & type) +{ + if (type != LaneChangeModuleType::NORMAL) { + return target_lanes; + } + + const auto target_lane = + utils::lane_change::getLaneChangeTargetLane(route_handler, current_lanes, type, direction); + if (!target_lane) { + return target_lanes; + } + + const auto itr = std::find_if( + target_lanes.begin(), target_lanes.end(), + [&](const lanelet::ConstLanelet & lane) { return lane.id() == target_lane->id(); }); + + if (itr == target_lanes.end()) { + return target_lanes; + } + + const int target_id = std::distance(target_lanes.begin(), itr); + const lanelet::ConstLanelets target_preferred_lanes( + target_lanes.begin() + target_id, target_lanes.end()); + return target_preferred_lanes; +} + +lanelet::ConstLanelets getTargetNeighborLanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType & type) +{ + lanelet::ConstLanelets neighbor_lanes; + + for (const auto & current_lane : current_lanes) { + if (route_handler.getNumLaneToPreferredLane(current_lane) != 0) { + if ( + type == LaneChangeModuleType::NORMAL || + type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { + neighbor_lanes.push_back(current_lane); + } + } else { + if (type != LaneChangeModuleType::NORMAL) { + neighbor_lanes.push_back(current_lane); + } + } + } + + return neighbor_lanes; +} + +lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType & type) +{ + 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()); + + return lanelet::utils::to2D(target_neighbor_preferred_lane_poly).basicPolygon(); +} + +bool isPathInLanelets( + const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) +{ + const auto current_lane_poly = + lanelet::utils::getPolygonFromArcLength(current_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly = + lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); + const auto current_lane_poly_2d = lanelet::utils::to2D(current_lane_poly).basicPolygon(); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_poly).basicPolygon(); + for (const auto & pt : path.points) { + const lanelet::BasicPoint2d ll_pt(pt.point.pose.position.x, pt.point.pose.position.y); + const auto is_in_current = boost::geometry::covered_by(ll_pt, current_lane_poly_2d); + if (is_in_current) { + continue; + } + const auto is_in_target = boost::geometry::covered_by(ll_pt, target_lane_poly_2d); + if (!is_in_target) { + return false; + } + } + return true; +} + +std::optional constructCandidatePath( + const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, + const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, + const std::vector> & sorted_lane_ids) +{ + const auto & shift_line = lane_change_info.shift_line; + const auto & original_lanes = lane_change_info.current_lanes; + const auto & target_lanes = lane_change_info.target_lanes; + const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; + const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; + const auto lane_change_velocity = lane_change_info.velocity; + + PathShifter path_shifter; + path_shifter.setPath(target_lane_reference_path); + path_shifter.addShiftLine(shift_line); + path_shifter.setLongitudinalAcceleration(longitudinal_acceleration.lane_changing); + ShiftedPath shifted_path; + + // offset front side + bool offset_back = false; + + const auto initial_lane_changing_velocity = lane_change_velocity.lane_changing; + path_shifter.setVelocity(initial_lane_changing_velocity); + path_shifter.setLateralAccelerationLimit(std::abs(lane_change_info.lateral_acceleration)); + + if (!path_shifter.generate(&shifted_path, offset_back)) { + RCLCPP_DEBUG(get_logger(), "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(get_logger(), "Path points are removed by PathShifter."); + return std::nullopt; + } + + LaneChangePath candidate_path; + candidate_path.info = lane_change_info; + + const auto lane_change_end_idx = + motion_utils::findNearestIndex(shifted_path.path.points, candidate_path.info.lane_changing_end); + + if (!lane_change_end_idx) { + RCLCPP_DEBUG(get_logger(), "Lane change end idx not found on target path."); + return std::nullopt; + } + + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *lane_change_end_idx) { + point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); + continue; + } + const auto nearest_idx = + motion_utils::findNearestIndex(target_segment.points, point.point.pose); + point.lane_ids = target_segment.points.at(*nearest_idx).lane_ids; + } + + // TODO(Yutaka Shimizu): remove this flag after make the isPathInLanelets faster + const bool enable_path_check_in_lanelet = false; + + // check candidate path is in lanelet + if ( + enable_path_check_in_lanelet && + !isPathInLanelets(shifted_path.path, original_lanes, target_lanes)) { + return std::nullopt; + } + + if (prepare_segment.points.size() > 1 && shifted_path.path.points.size() > 1) { + const auto & prepare_segment_second_last_point = + std::prev(prepare_segment.points.end() - 1)->point.pose; + const auto & lane_change_start_from_shifted = + std::next(shifted_path.path.points.begin())->point.pose; + const auto yaw_diff2 = std::abs(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(prepare_segment_second_last_point.orientation) - + tf2::getYaw(lane_change_start_from_shifted.orientation))); + if (yaw_diff2 > tier4_autoware_utils::deg2rad(5.0)) { + RCLCPP_DEBUG( + get_logger(), "Excessive yaw difference %.3f which exceeds the 5 degrees threshold.", + tier4_autoware_utils::rad2deg(yaw_diff2)); + return std::nullopt; + } + } + + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); + candidate_path.shifted_path = shifted_path; + + return std::optional{candidate_path}; +} + +PathWithLaneId getReferencePathFromTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & lane_changing_start_pose, const double target_lane_length, + const double lane_changing_length, const double forward_path_length, + const double resample_interval, const bool is_goal_in_route, const double next_lane_change_buffer) +{ + const ArcCoordinates lane_change_start_arc_position = + lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); + + const double s_start = lane_change_start_arc_position.length; + const double s_end = std::invoke([&]() { + const auto dist_from_lc_start = s_start + lane_changing_length + forward_path_length; + if (is_goal_in_route) { + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length - + next_lane_change_buffer; + return std::min(dist_from_lc_start, s_goal); + } + return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); + }); + + constexpr double epsilon = 1e-4; + if (s_end - s_start + epsilon < lane_changing_length) { + return PathWithLaneId(); + } + + const auto lane_changing_reference_path = + route_handler.getCenterLinePath(target_lanes, s_start, s_end); + + return utils::resamplePathWithSpline( + lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); +} + +ShiftLine getLaneChangingShiftLine( + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const PathWithLaneId & reference_path, const double shift_length) +{ + const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; + + return getLaneChangingShiftLine( + lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length); +} + +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length) +{ + ShiftLine shift_line; + shift_line.end_shift_length = shift_length; + shift_line.start = lane_changing_start_pose; + shift_line.end = lane_changing_end_pose; + shift_line.start_idx = + motion_utils::findNearestIndex(reference_path.points, lane_changing_start_pose.position); + shift_line.end_idx = + motion_utils::findNearestIndex(reference_path.points, lane_changing_end_pose.position); + + return shift_line; +} + +std::vector generateDrivableLanes( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & lane_change_lanes) +{ + size_t current_lc_idx = 0; + std::vector drivable_lanes(current_lanes.size()); + for (size_t i = 0; i < current_lanes.size(); ++i) { + const auto & current_lane = current_lanes.at(i); + drivable_lanes.at(i).left_lane = current_lane; + drivable_lanes.at(i).right_lane = current_lane; + + const auto left_lane = route_handler.getLeftLanelet(current_lane, false, false); + const auto right_lane = route_handler.getRightLanelet(current_lane, false, false); + if (!left_lane && !right_lane) { + continue; + } + + for (size_t lc_idx = current_lc_idx; lc_idx < lane_change_lanes.size(); ++lc_idx) { + const auto & lc_lane = lane_change_lanes.at(lc_idx); + if (left_lane && lc_lane.id() == left_lane->id()) { + drivable_lanes.at(i).left_lane = lc_lane; + current_lc_idx = lc_idx; + break; + } + + if (right_lane && lc_lane.id() == right_lane->id()) { + drivable_lanes.at(i).right_lane = lc_lane; + current_lc_idx = lc_idx; + break; + } + } + } + + for (size_t i = current_lc_idx + 1; i < lane_change_lanes.size(); ++i) { + const auto & lc_lane = lane_change_lanes.at(i); + DrivableLanes drivable_lane; + drivable_lane.left_lane = lc_lane; + drivable_lane.right_lane = lc_lane; + drivable_lanes.push_back(drivable_lane); + } + + return drivable_lanes; +} + +std::vector generateDrivableLanes( + const std::vector original_drivable_lanes, const RouteHandler & route_handler, + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes) +{ + 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(); + }; + + const auto check_middle = [&](const auto & lane) -> std::optional { + for (const auto & drivable_lane : original_drivable_lanes) { + if (has_same_lane(drivable_lane.middle_lanes, lane)) { + return drivable_lane; + } + } + return std::nullopt; + }; + + const auto check_left = [&](const auto & lane) -> std::optional { + for (const auto & drivable_lane : original_drivable_lanes) { + if (drivable_lane.left_lane.id() == lane.id()) { + return drivable_lane; + } + } + return std::nullopt; + }; + + const auto check_right = [&](const auto & lane) -> std::optional { + for (const auto & drivable_lane : original_drivable_lanes) { + if (drivable_lane.right_lane.id() == lane.id()) { + return drivable_lane; + } + } + return std::nullopt; + }; + + size_t current_lc_idx = 0; + std::vector drivable_lanes(current_lanes.size()); + for (size_t i = 0; i < current_lanes.size(); ++i) { + const auto & current_lane = current_lanes.at(i); + + const auto middle_drivable_lane = check_middle(current_lane); + if (middle_drivable_lane) { + drivable_lanes.at(i) = *middle_drivable_lane; + } + + const auto left_drivable_lane = check_left(current_lane); + if (left_drivable_lane) { + drivable_lanes.at(i) = *left_drivable_lane; + } + + const auto right_drivable_lane = check_right(current_lane); + if (right_drivable_lane) { + drivable_lanes.at(i) = *right_drivable_lane; + } + + if (!middle_drivable_lane && !left_drivable_lane && !right_drivable_lane) { + drivable_lanes.at(i).left_lane = current_lane; + drivable_lanes.at(i).right_lane = current_lane; + } + + const auto left_lane = route_handler.getLeftLanelet(current_lane); + const auto right_lane = route_handler.getRightLanelet(current_lane); + if (!left_lane && !right_lane) { + continue; + } + + for (size_t lc_idx = current_lc_idx; lc_idx < lane_change_lanes.size(); ++lc_idx) { + const auto & lc_lane = lane_change_lanes.at(lc_idx); + if (left_lane && lc_lane.id() == left_lane->id()) { + if (left_drivable_lane) { + drivable_lanes.at(i).left_lane = lc_lane; + } + current_lc_idx = lc_idx; + break; + } + + if (right_lane && lc_lane.id() == right_lane->id()) { + if (right_drivable_lane) { + drivable_lanes.at(i).right_lane = lc_lane; + } + current_lc_idx = lc_idx; + break; + } + } + } + + for (size_t i = current_lc_idx + 1; i < lane_change_lanes.size(); ++i) { + const auto & lc_lane = lane_change_lanes.at(i); + DrivableLanes drivable_lane; + + const auto middle_drivable_lane = check_middle(lc_lane); + if (middle_drivable_lane) { + drivable_lane = *middle_drivable_lane; + } + + const auto left_drivable_lane = check_left(lc_lane); + if (left_drivable_lane) { + drivable_lane = *left_drivable_lane; + } + + const auto right_drivable_lane = check_right(lc_lane); + if (right_drivable_lane) { + drivable_lane = *right_drivable_lane; + } + + if (!middle_drivable_lane && !left_drivable_lane && !right_drivable_lane) { + drivable_lane.left_lane = lc_lane; + drivable_lane.right_lane = lc_lane; + } + + drivable_lanes.push_back(drivable_lane); + } + + return drivable_lanes; +} + +double getLateralShift(const LaneChangePath & path) +{ + if (path.shifted_path.shift_length.empty()) { + return 0.0; + } + + const auto start_idx = + std::min(path.info.shift_line.start_idx, path.shifted_path.shift_length.size() - 1); + const auto end_idx = + std::min(path.info.shift_line.end_idx, path.shifted_path.shift_length.size() - 1); + + return path.shifted_path.shift_length.at(end_idx) - path.shifted_path.shift_length.at(start_idx); +} + +bool hasEnoughLengthToLaneChangeAfterAbort( + const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, + const Pose & current_pose, const double abort_return_dist, + const LaneChangeParameters & lane_change_parameters, const Direction direction) +{ + if (current_lanes.empty()) { + return false; + } + + const auto minimum_lane_change_length = calcMinimumLaneChangeLength( + route_handler, current_lanes.back(), lane_change_parameters, direction); + const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; + if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + return false; + } + + if ( + abort_plus_lane_change_length > + utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)) { + return false; + } + + return true; +} + +double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer) +{ + return lateral_buffer + 0.5 * vehicle_width; +} + +std::string getStrDirection(const std::string & name, const Direction direction) +{ + if (direction == Direction::LEFT) { + return name + "_left"; + } + if (direction == Direction::RIGHT) { + return name + "_right"; + } + return ""; +} + +std::vector> getSortedLaneIds( + const RouteHandler & route_handler, const Pose & current_pose, + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) +{ + const auto rough_shift_length = + lanelet::utils::getArcCoordinates(target_lanes, current_pose).distance; + + std::vector> sorted_lane_ids{}; + sorted_lane_ids.reserve(target_lanes.size()); + const auto get_sorted_lane_ids = [&](const lanelet::ConstLanelet & target_lane) { + const auto routing_graph_ptr = route_handler.getRoutingGraphPtr(); + lanelet::ConstLanelet lane; + if (rough_shift_length < 0.0) { + // lane change to the left, so i wan to take the lane right to target + const auto has_target_right = routing_graph_ptr->right(target_lane); + if (has_target_right) { + lane = *has_target_right; + } + } else if (rough_shift_length > 0.0) { + const auto has_target_left = routing_graph_ptr->left(target_lane); + if (has_target_left) { + lane = *has_target_left; + } + } else { + lane = target_lane; + } + + const auto find_same_id = std::find_if( + current_lanes.cbegin(), current_lanes.cend(), + [&lane](const lanelet::ConstLanelet & orig) { return orig.id() == lane.id(); }); + + if (find_same_id == current_lanes.cend()) { + return std::vector{target_lane.id()}; + } + + if (target_lane.id() > find_same_id->id()) { + return std::vector{find_same_id->id(), target_lane.id()}; + } + + return std::vector{target_lane.id(), find_same_id->id()}; + }; + + std::transform( + target_lanes.cbegin(), target_lanes.cend(), std::back_inserter(sorted_lane_ids), + get_sorted_lane_ids); + + return sorted_lane_ids; +} + +std::vector replaceWithSortedIds( + const std::vector & original_lane_ids, + const std::vector> & sorted_lane_ids) +{ + for (const auto original_id : original_lane_ids) { + for (const auto & sorted_id : sorted_lane_ids) { + if (std::find(sorted_id.cbegin(), sorted_id.cend(), original_id) != sorted_id.cend()) { + return sorted_id; + } + } + } + return original_lane_ids; +} + +CandidateOutput assignToCandidate( + const LaneChangePath & lane_change_path, const Point & ego_position) +{ + CandidateOutput candidate_output; + candidate_output.path_candidate = lane_change_path.path; + candidate_output.lateral_shift = utils::lane_change::getLateralShift(lane_change_path); + candidate_output.start_distance_to_path_change = motion_utils::calcSignedArcLength( + lane_change_path.path.points, ego_position, lane_change_path.info.shift_line.start.position); + candidate_output.finish_distance_to_path_change = motion_utils::calcSignedArcLength( + lane_change_path.path.points, ego_position, lane_change_path.info.shift_line.end.position); + + return candidate_output; +} + +std::optional getLaneChangeTargetLane( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const LaneChangeModuleType type, const Direction & direction) +{ + if ( + type == LaneChangeModuleType::NORMAL || + type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { + return route_handler.getLaneChangeTarget(current_lanes, direction); + } + + return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); +} + +std::vector convertToPredictedPath( + const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose, + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const double resolution) +{ + if (lane_change_path.path.points.empty()) { + return {}; + } + + const auto & path = lane_change_path.path; + const auto prepare_acc = lane_change_path.info.longitudinal_acceleration.prepare; + const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; + const auto duration = lane_change_path.info.duration.sum(); + const auto prepare_time = lane_change_path.info.duration.prepare; + const auto & minimum_lane_changing_velocity = + lane_change_parameters.minimum_lane_changing_velocity; + + const auto nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + + std::vector predicted_path; + const auto vehicle_pose_frenet = + convertToFrenetPoint(path.points, vehicle_pose.position, nearest_seg_idx); + const double initial_velocity = std::abs(vehicle_twist.linear.x); + + // prepare segment + for (double t = 0.0; t < prepare_time; t += resolution) { + const double velocity = + std::max(initial_velocity + prepare_acc * t, minimum_lane_changing_velocity); + const double length = initial_velocity * t + 0.5 * prepare_acc * t * t; + const auto pose = + motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + // lane changing segment + const double lane_changing_velocity = + std::max(initial_velocity + prepare_acc * prepare_time, minimum_lane_changing_velocity); + const double offset = + initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time; + for (double t = prepare_time; t < duration; t += resolution) { + const double delta_t = t - prepare_time; + const double velocity = lane_changing_velocity + lane_changing_acc * delta_t; + const double length = + lane_changing_velocity * delta_t + 0.5 * lane_changing_acc * delta_t * delta_t + offset; + const auto pose = + motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + return predicted_path; +} + +bool isParkedObject( + const PathWithLaneId & path, const RouteHandler & route_handler, + const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, + const double object_shiftable_ratio_threshold, const double static_object_velocity_threshold) +{ + // ============================================ <- most_left_lanelet.leftBound() + // y road shoulder + // ^ ------------------------------------------ + // | x + + // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() + // + // -------------------------------------------- + // +: object position + // o: nearest point on centerline + + using lanelet::geometry::distance2d; + using lanelet::geometry::toArcCoordinates; + + const double object_vel_norm = + std::hypot(object.initial_twist.twist.linear.x, object.initial_twist.twist.linear.y); + if (object_vel_norm > static_object_velocity_threshold) { + return false; + } + + const auto & object_pose = object.initial_pose.pose; + const auto object_closest_index = + motion_utils::findNearestIndex(path.points, object_pose.position); + const auto object_closest_pose = path.points.at(object_closest_index).point.pose; + + lanelet::ConstLanelet closest_lanelet; + if (!route_handler.getClosestLaneletWithinRoute(object_closest_pose, &closest_lanelet)) { + return false; + } + + const double lat_dist = motion_utils::calcLateralOffset(path.points, object_pose.position); + lanelet::BasicLineString2d bound; + double center_to_bound_buffer = 0.0; + if (lat_dist > 0.0) { + // left side vehicle + const auto most_left_road_lanelet = route_handler.getMostLeftLanelet(closest_lanelet); + const auto most_left_lanelet_candidates = + route_handler.getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); + lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; + const lanelet::Attribute sub_type = + most_left_lanelet.attribute(lanelet::AttributeName::Subtype); + + for (const auto & ll : most_left_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_left_lanelet = ll; + } + } + bound = most_left_lanelet.leftBound2d().basicLineString(); + if (sub_type.value() != "road_shoulder") { + center_to_bound_buffer = object_check_min_road_shoulder_width; + } + } else { + // right side vehicle + const auto most_right_road_lanelet = route_handler.getMostRightLanelet(closest_lanelet); + const auto most_right_lanelet_candidates = + route_handler.getLaneletMapPtr()->laneletLayer.findUsages( + most_right_road_lanelet.rightBound()); + + lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; + const lanelet::Attribute sub_type = + most_right_lanelet.attribute(lanelet::AttributeName::Subtype); + + for (const auto & ll : most_right_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_right_lanelet = ll; + } + } + bound = most_right_lanelet.rightBound2d().basicLineString(); + if (sub_type.value() != "road_shoulder") { + center_to_bound_buffer = object_check_min_road_shoulder_width; + } + } + + return isParkedObject( + closest_lanelet, bound, object, center_to_bound_buffer, object_shiftable_ratio_threshold); +} + +bool isParkedObject( + const lanelet::ConstLanelet & closest_lanelet, const lanelet::BasicLineString2d & boundary, + const ExtendedPredictedObject & object, const double buffer_to_bound, + const double ratio_threshold) +{ + using lanelet::geometry::distance2d; + + const auto & obj_pose = object.initial_pose.pose; + const auto & obj_shape = object.shape; + const auto obj_poly = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); + const auto obj_point = obj_pose.position; + + double max_dist_to_bound = std::numeric_limits::lowest(); + double min_dist_to_bound = std::numeric_limits::max(); + for (const auto & edge : obj_poly.outer()) { + const auto ll_edge = lanelet::Point2d(lanelet::InvalId, edge.x(), edge.y()); + const auto dist = distance2d(boundary, ll_edge); + max_dist_to_bound = std::max(dist, max_dist_to_bound); + min_dist_to_bound = std::min(dist, min_dist_to_bound); + } + const double obj_width = std::max(max_dist_to_bound - min_dist_to_bound, 0.0); + + // distance from centerline to the boundary line with object width + const auto centerline_pose = lanelet::utils::getClosestCenterPose(closest_lanelet, obj_point); + const lanelet::BasicPoint3d centerline_point( + centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); + const double dist_bound_to_centerline = + std::abs(distance2d(boundary, centerline_point)) - 0.5 * obj_width + buffer_to_bound; + + // distance from object point to centerline + const auto centerline = closest_lanelet.centerline(); + const auto ll_obj_point = lanelet::Point2d(lanelet::InvalId, obj_point.x, obj_point.y); + const double dist_obj_to_centerline = std::abs(distance2d(centerline, ll_obj_point)); + + const double ratio = dist_obj_to_centerline / std::max(dist_bound_to_centerline, 1e-6); + const double clamped_ratio = std::clamp(ratio, 0.0, 1.0); + return clamped_ratio > ratio_threshold; +} + +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, + CollisionCheckDebugMap & object_debug) +{ + const auto & object_check_min_road_shoulder_width = + lane_change_parameters.object_check_min_road_shoulder_width; + const auto & object_shiftable_ratio_threshold = + lane_change_parameters.object_shiftable_ratio_threshold; + const auto & path = lane_change_path.path; + const auto & current_lanes = lane_change_path.info.current_lanes; + const auto current_lane_path = + route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + + if (objects.empty() || path.points.empty() || current_lane_path.points.empty()) { + return false; + } + + const auto leading_obj_idx = getLeadingStaticObjectIdx( + route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, + object_shiftable_ratio_threshold); + if (!leading_obj_idx) { + return false; + } + + const auto & leading_obj = objects.at(*leading_obj_idx); + auto debug = utils::path_safety_checker::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()) { + return false; + } + + const auto & current_path_end = current_lane_path.points.back().point.pose.position; + double min_dist_to_end_of_current_lane = std::numeric_limits::max(); + for (const auto & point : leading_obj_poly.outer()) { + const auto obj_p = tier4_autoware_utils::createPoint(point.x(), point.y(), 0.0); + const double dist = + motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, current_path_end); + min_dist_to_end_of_current_lane = std::min(dist, min_dist_to_end_of_current_lane); + if (is_goal_in_route) { + const auto goal_pose = route_handler.getGoalPose(); + const double dist_to_goal = + motion_utils::calcSignedArcLength(current_lane_path.points, obj_p, goal_pose.position); + min_dist_to_end_of_current_lane = std::min(min_dist_to_end_of_current_lane, dist_to_goal); + } + } + + // 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"; + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); + return true; + } + + return false; +} + +std::optional getLeadingStaticObjectIdx( + const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const std::vector & objects, + const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold) +{ + const auto & path = lane_change_path.path; + + if (path.points.empty() || objects.empty()) { + return {}; + } + + const auto & lane_change_start = lane_change_path.info.lane_changing_start; + const auto & path_end = path.points.back(); + + double dist_lc_start_to_leading_obj = 0.0; + std::optional leading_obj_idx = std::nullopt; + for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { + const auto & obj = objects.at(obj_idx); + const auto & obj_pose = obj.initial_pose.pose; + + // ignore non-static object + // TODO(shimizu): parametrize threshold + const double obj_vel_norm = + std::hypot(obj.initial_twist.twist.linear.x, obj.initial_twist.twist.linear.y); + if (obj_vel_norm > 1.0) { + continue; + } + + // ignore non-parked object + if (!isParkedObject( + path, route_handler, obj, object_check_min_road_shoulder_width, + object_shiftable_ratio_threshold)) { + continue; + } + + const double dist_back_to_obj = motion_utils::calcSignedArcLength( + path.points, path_end.point.pose.position, obj_pose.position); + if (dist_back_to_obj > 0.0) { + // object is not on the lane change path + continue; + } + + const double dist_lc_start_to_obj = + motion_utils::calcSignedArcLength(path.points, lane_change_start.position, obj_pose.position); + if (dist_lc_start_to_obj < 0.0) { + // object is on the lane changing path or behind it. It will be detected in safety check + continue; + } + + if (dist_lc_start_to_obj > dist_lc_start_to_leading_obj) { + dist_lc_start_to_leading_obj = dist_lc_start_to_obj; + leading_obj_idx = obj_idx; + } + } + + return leading_obj_idx; +} + +std::optional createPolygon( + const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist) +{ + if (lanes.empty()) { + return {}; + } + const auto polygon_3d = lanelet::utils::getPolygonFromArcLength(lanes, start_dist, end_dist); + return lanelet::utils::to2D(polygon_3d).basicPolygon(); +} + +ExtendedPredictedObject transform( + const PredictedObject & object, + [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase) +{ + ExtendedPredictedObject extended_object(object); + + const auto & time_resolution = lane_change_parameters.prediction_time_resolution; + const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; + const auto & velocity_threshold = + lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; + const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; + const double obj_vel_norm = std::hypot( + extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); + + 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); + const double end_time = + rclcpp::Duration(path.time_step).seconds() * static_cast(path.path.size() - 1); + extended_object.predicted_paths.at(i).confidence = path.confidence; + + // create path + for (double t = start_time; t < end_time + std::numeric_limits::epsilon(); + t += time_resolution) { + if (t < prepare_duration && obj_vel_norm < velocity_threshold) { + continue; + } + 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_vel_norm, obj_polygon); + } + } + } + + 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); +} + +rclcpp::Logger getLogger(const std::string & type) +{ + return rclcpp::get_logger("lane_change").get_child(type); +} + +Polygon2d getEgoCurrentFootprint( + const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info) +{ + const auto base_to_front = ego_info.max_longitudinal_offset_m; + const auto base_to_rear = ego_info.rear_overhang_m; + const auto width = ego_info.vehicle_width_m; + + return tier4_autoware_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); +} + +bool isWithinIntersection( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, + const Polygon2d & polygon) +{ + const std::string id = lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { + return false; + } + + const auto lanelet_polygon = + route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + + return boost::geometry::within( + polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon()))); +} + +bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) +{ + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "else" || turn_direction == "straight") { + return false; + } + + return !boost::geometry::disjoint( + polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet.polygon2d().basicPolygon()))); +} + +double calcPhaseLength( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration) +{ + const auto length_with_acceleration = + velocity * duration + 0.5 * acceleration * std::pow(duration, 2); + const auto length_with_max_velocity = maximum_velocity * duration; + return std::min(length_with_acceleration, length_with_max_velocity); +} + +LanesPolygon createLanesPolygon( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const std::vector & target_backward_lanes) +{ + LanesPolygon lanes_polygon; + + lanes_polygon.current = + utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); + lanes_polygon.target = + utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + + for (const auto & target_backward_lane : target_backward_lanes) { + auto lane_polygon = utils::lane_change::createPolygon( + target_backward_lane, 0.0, std::numeric_limits::max()); + + if (lane_polygon) { + lanes_polygon.target_backward.push_back(*lane_polygon); + } + } + return lanes_polygon; +} +} // namespace autoware::behavior_path_planner::utils::lane_change + +namespace autoware::behavior_path_planner::utils::lane_change::debug +{ +geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Point32 p; + p.x = static_cast(pose.position.x); + p.y = static_cast(pose.position.y); + p.z = static_cast(pose.position.z); + return p; +}; + +geometry_msgs::msg::Polygon createExecutionArea( + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, + double additional_lon_offset, double additional_lat_offset) +{ + 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; + + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + additional_lon_offset; + const double backward_lon_offset = -base_to_rear; + const double lat_offset = width / 2.0 + additional_lat_offset; + + const auto p1 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + geometry_msgs::msg::Polygon polygon; + + polygon.points.push_back(create_point32(p1)); + polygon.points.push_back(create_point32(p2)); + polygon.points.push_back(create_point32(p3)); + polygon.points.push_back(create_point32(p4)); + + return polygon; +} + +} // namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..2dd5bb2c55063 --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,126 @@ +// 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 "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware_planning_test_manager/autoware_planning_test_manager.hpp" +#include "planning_test_utils/planning_test_utils.hpp" + +#include + +#include +#include + +using autoware::behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_path_lane_change_module"); + + std::vector module_names; + module_names.emplace_back("autoware::behavior_path_planner::LaneChangeRightModuleManager"); + module_names.emplace_back("autoware::behavior_path_planner::LaneChangeLeftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp b/planning/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp similarity index 95% rename from planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp rename to planning/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 1f90114df8045..37040e0a8c275 100644 --- a/planning/behavior_path_lane_change_module/test/test_lane_change_utils.cpp +++ b/planning/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp @@ -11,7 +11,7 @@ // 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_lane_change_module/utils/data_structs.hpp" +#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" #include #include @@ -41,7 +41,7 @@ TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) TEST(BehaviorPathPlanningLaneChangeUtilsTest, TESTLateralAccelerationMap) { - behavior_path_planner::LateralAccelerationMap lat_acc_map; + autoware::behavior_path_planner::LateralAccelerationMap lat_acc_map; lat_acc_map.add(0.0, 0.2, 0.315); lat_acc_map.add(3.0, 0.2, 0.315); lat_acc_map.add(5.0, 0.2, 0.315); diff --git a/planning/autoware_behavior_path_planner/CMakeLists.txt b/planning/autoware_behavior_path_planner/CMakeLists.txt new file mode 100644 index 0000000000000..cadaacd01d9c9 --- /dev/null +++ b/planning/autoware_behavior_path_planner/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_path_planner) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(OpenCV REQUIRED) +find_package(magic_enum CONFIG REQUIRED) + +ament_auto_add_library(${PROJECT_NAME}_lib SHARED + src/planner_manager.cpp + src/behavior_path_planner_node.cpp +) + +target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC + ${EIGEN3_INCLUDE_DIR} +) + +target_link_libraries(${PROJECT_NAME}_lib + ${OpenCV_LIBRARIES} +) + +rclcpp_components_register_node(${PROJECT_NAME}_lib + PLUGIN "autoware::behavior_path_planner::BehaviorPathPlannerNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_utilities + test/input.cpp + test/test_utils.cpp + ) + + target_link_libraries(test_${CMAKE_PROJECT_NAME}_utilities + ${PROJECT_NAME}_lib + ) + + ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node_interface + test/test_${PROJECT_NAME}_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_node_interface + ${PROJECT_NAME}_lib + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + config + launch +) diff --git a/planning/autoware_behavior_path_planner/README.md b/planning/autoware_behavior_path_planner/README.md new file mode 100644 index 0000000000000..658edce2ccb3c --- /dev/null +++ b/planning/autoware_behavior_path_planner/README.md @@ -0,0 +1,283 @@ +# Behavior Path Planner + +The Behavior Path Planner's main objective is to significantly enhance the safety of autonomous vehicles by minimizing the risk of accidents. It improves driving efficiency through time conservation and underpins reliability with its rule-based approach. Additionally, it allows users to integrate their own custom behavior modules or use it with different types of vehicles, such as cars, buses, and delivery robots, as well as in various environments, from busy urban streets to open highways. + +The module begins by thoroughly analyzing the ego vehicle's current situation, including its position, speed, and surrounding environment. This analysis leads to essential driving decisions about lane changes or stopping and subsequently generates a path that is both safe and efficient. It considers road geometry, traffic rules, and dynamic conditions while also incorporating obstacle avoidance to respond to static and dynamic obstacles such as other vehicles, pedestrians, or unexpected roadblocks, ensuring safe navigation. + +Moreover, the planner actively interacts with other traffic participants, predicting their actions and accordingly adjusting the vehicle's path. This ensures not only the safety of the autonomous vehicle but also contributes to smooth traffic flow. Its adherence to traffic laws, including speed limits and traffic signals, further guarantees lawful and predictable driving behavior. The planner is also designed to minimize sudden or abrupt maneuvers, aiming for a comfortable and natural driving experience. + +!!! note + + The [Planning Component Design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/) Document outlines the foundational philosophy guiding the design and future development of the Behavior Path Planner module. We strongly encourage readers to consult this document to understand the rationale behind its current configuration and the direction of its ongoing development. + +## Purpose / Use Cases + +Essentially, the module has three primary responsibilities: + +1. Creating a **path based** on the traffic situation. +2. Generating **drivable area**, i.e. the area within which the vehicle can maneuver. +3. Generating **turn signal** commands to be relayed to the vehicle interface. + +## Features + +### Supported Scene Modules + +Behavior Path Planner has following scene modules + +| Name | Description | Details | +| :------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------------- | +| Lane Following | this module generates reference path from lanelet centerline. | LINK | +| Static Obstacle Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) | +| Dynamic Obstacle Avoidance | WIP | [LINK](../autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md) | +| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | +| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../autoware_behavior_path_lane_change_module/README.md) | +| External Lane Change | WIP | LINK | +| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../autoware_behavior_path_goal_planner_module/README.md) | +| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../autoware_behavior_path_start_planner_module/README.md) | +| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../autoware_behavior_path_side_shift_module/README.md) | + +!!! Note + + click on the following images to view the video of their execution + +
+ + + + + + + + + + + +
Lane Following ModuleAvoidance ModuleAvoidance by Lane Change Module
Lane Change ModuleStart Planner ModuleGoal Planner Module
+
+ +!!! Note + + Users can refer to [Planning component design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/#supported-functions) for some additional behavior. + +#### How to add or implement new module? + +All scene modules are implemented by inheriting base class `scene_module_interface.hpp`. + +!!! Warning + + The remainder of this subsection is work in progress (WIP). + +### Planner Manager + +The Planner Manager's responsibilities include: + +1. Activating the relevant scene module in response to the specific situation faced by the autonomous vehicle. For example, when a parked vehicle blocks the ego vehicle's driving lane, the manager would engage the avoidance module. +2. Managing the execution order when multiple modules are running simultaneously. For instance, if both the lane-changing and avoidance modules are operational, the manager decides which should take precedence. +3. Merging paths from multiple modules when they are activated simultaneously and each generates its own path, thereby creating a single functional path. + +!!! note + + To check the scene module's transition, i.e.: registered, approved and candidate modules, set `verbose: true` in the [behavior path planner configuration file](https://github.com/autowarefoundation/autoware_launch/blob/0cd5d891a36ac34a32a417205905c109f2bafe7b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml#L3). + + ![Scene module's transition table](./image/checking_module_transition.png) + +!!! note + + For more in-depth information, refer to [Manager design](./docs/behavior_path_planner_manager_design.md) document. + +## Inputs / Outputs / API + +### Input + +| Name | Required? | Type | Description | +| :---------------------------- | :-------: | :------------------------------------------------------ | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | +| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | +| ~/input/objects | ○ | `autoware_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | +| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | +| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | traffic signals information from the perception module | +| ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | vector map information. | +| ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | +| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | +| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | +| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/operation-mode.md) | + +- ○ Mandatory: Planning Module would not work if anyone of this is not present. +- △ Optional: Some module would not work, but Planning Module can still be operated. + +### Output + +| Name | Type | Description | QoS Durability | +| :---------------------------- | :-------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | +| ~/output/path | `tier4_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | +| ~/output/turn_indicators_cmd | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | `volatile` | +| ~/output/hazard_lights_cmd | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command. | `volatile` | +| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | output modified goal commands. | `transient_local` | +| ~/output/stop_reasons | `tier4_planning_msgs::msg::StopReasonArray` | describe the reason for ego vehicle stop | `volatile` | +| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | + +### Debug + +| Name | Type | Description | QoS Durability | +| :-------------------------------------- | :-------------------------------------------------- | :---------------------------------------------------------------------------------------- | -------------- | +| ~/debug/avoidance_debug_message_array | `tier4_planning_msgs::msg::AvoidanceDebugMsgArray` | debug message for avoidance. notify users reasons for avoidance path cannot be generated. | `volatile` | +| ~/debug/lane_change_debug_message_array | `tier4_planning_msgs::msg::LaneChangeDebugMsgArray` | debug message for lane change. notify users unsafe reason during lane changing process | `volatile` | +| ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | shows maximum static drivable area. | `volatile` | +| ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | +| ~/debug/bound | `visualization_msgs::msg::MarkerArray` | debug for static drivable area | `volatile` | +| ~/planning/path_candidate/\* | `autoware_planning_msgs::msg::Path` | the path before approval. | `volatile` | +| ~/planning/path_reference/\* | `autoware_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | + +!!! note + + For specific information of which topics are being subscribed and published, refer to [behavior_path_planner.xml](https://github.com/autowarefoundation/autoware.universe/blob/9000f430c937764c14e43109539302f1f878ed70/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml#L36-L49). + +## How to enable or disable the modules + +Enabling and disabling the modules in the behavior path planner is primarily managed through two key files: `default_preset.yaml` and `behavior_path_planner.launch.xml`. + +The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the behavior path planner's modules or features. For example: + +- `launch_static_obstacle_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. + +!!! note + + Click [here](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/preset/default_preset.yaml) to view the `default_preset.yaml`. + +The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `static_obstacle_avoidance.enable_module` in + +```xml + +``` + +corresponds to launch_static_obstacle_avoidance_module from `default_preset.yaml`. + +Therefore, to enable or disable a module, simply set the corresponding module in `default_preset.yaml` to `true` or `false`. These changes will be applied upon the next launch of Autoware. + +## Generating Path + +A sophisticated methodology is used for path generation, particularly focusing on maneuvers like lane changes and avoidance. At the core of this design is the smooth lateral shifting of the reference path, achieved through a constant-jerk profile. This approach ensures a consistent rate of change in acceleration, facilitating smooth transitions and minimizing abrupt changes in lateral dynamics, crucial for passenger comfort and safety. + +The design involves complex mathematical formulations for calculating the lateral shift of the vehicle's path over time. These calculations include determining lateral displacement, velocity, and acceleration, while considering the vehicle's lateral acceleration and velocity limits. This is essential for ensuring that the vehicle's movements remain safe and manageable. + +The `ShiftLine` struct (as seen [here](https://github.com/autowarefoundation/autoware.universe/blob/9000f430c937764c14e43109539302f1f878ed70/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp#L35-L48)) is utilized to represent points along the path where the lateral shift starts and ends. It includes details like the start and end points in absolute coordinates, the relative shift lengths at these points compared to the reference path, and the associated indexes on the reference path. This struct is integral to managing the path shifts, as it allows the path planner to dynamically adjust the trajectory based on the vehicle's current position and planned maneuver. + +Furthermore, the design and its implementation incorporate various equations and mathematical models to calculate essential parameters for the path shift. These include the total distance of the lateral shift, the maximum allowable lateral acceleration and jerk, and the total time required for the shift. Practical considerations are also noted, such as simplifying assumptions in the absence of a specific time interval for most lane change and avoidance cases. + +The shifted path generation logic enables the behavior path planner to dynamically generate safe and efficient paths, precisely controlling the vehicle’s lateral movements to ensure the smooth execution of lane changes and avoidance maneuvers. This careful planning and execution adhere to the vehicle's dynamic capabilities and safety constraints, maximizing efficiency and safety in autonomous vehicle navigation. + +!!! note + + If you're a math lover, refer to [Path Generation Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md) for the nitty-gritty. + +## Collision Assessment / Safety check + +The purpose of the collision assessment function in the Behavior Path Planner is to evaluate the potential for collisions with target objects across all modules. It is utilized in two scenarios: + +1. During candidate path generation, to ensure that the generated candidate path is collision-free. +2. When the path is approved by the manager, and the ego vehicle is executing the current module. If the current situation is deemed unsafe, depending on each module's requirements, the planner will either cancel the execution or opt to execute another module. + +The safety check process involves several steps. Initially, it obtains the pose of the target object at a specific time, typically through interpolation of the predicted path. It then checks for any overlap between the ego vehicle and the target object at this time. If an overlap is detected, the path is deemed unsafe. The function also identifies which vehicle is in front by using the arc length along the given path. The function operates under the assumption that accurate data on the position, velocity, and shape of both the ego vehicle (the autonomous vehicle) and any target objects are available. It also relies on the yaw angle of each point in the predicted paths of these objects, which is expected to point towards the next path point. + +A critical part of the safety check is the calculation of the RSS (Responsibility-Sensitive Safety) distance-inspired algorithm. This algorithm considers factors such as reaction time, safety time margin, and the velocities and decelerations of both vehicles. Extended object polygons are created for both the ego and target vehicles. Notably, the rear object’s polygon is extended by the RSS distance longitudinally and by a lateral margin. The function finally checks for overlap between this extended rear object polygon and the front object polygon. Any overlap indicates a potential unsafe situation. + +However, the module does have a limitation concerning the yaw angle of each point in the predicted paths of target objects, which may not always accurately point to the next point, leading to potential inaccuracies in some edge cases. + +!!! note + + For further reading on the collision assessment method, please refer to [Safety check utils](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) + +## Generating Drivable Area + +### Static Drivable Area logic + +The drivable area is used to determine the area in which the ego vehicle can travel. The primary goal of static drivable area expansion is to ensure safe travel by generating an area that encompasses only the necessary spaces for the vehicle's current behavior, while excluding non-essential areas. For example, while `avoidance` module is running, the drivable area includes additional space needed for maneuvers around obstacles, and it limits the behavior by not extending the avoidance path outside of lanelet areas. + +
+ + + + + + + +
Before expansion
After expansion
+
+ +Static drivable area expansion operates under assumptions about the correct arrangement of lanes and the coverage of both the front and rear of the vehicle within the left and right boundaries. Key parameters for drivable area generation include extra footprint offsets for the ego vehicle, the handling of dynamic objects, maximum expansion distance, and specific methods for expansion. Additionally, since each module generates its own drivable area, before passing it as the input to generate the next running module's drivable area, or before generating a unified drivable area, the system sorts drivable lanes based on the vehicle's passage order. This ensures the correct definition of the lanes used in drivable area generation. + +!!! note + + Further details can is provided in [Drivable Area Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). + +### Dynamic Drivable Area Logic + +Large vehicles require much more space, which sometimes causes them to veer out of their current lane. A typical example being a bus making a turn at a corner. In such cases, relying on a static drivable area is insufficient, since the static method depends on lane information provided by high-definition maps. To overcome the limitations of the static approach, the dynamic drivable area expansion algorithm adjusts the navigable space for an autonomous vehicle in real-time. It conserves computational power by reusing previously calculated path data, updating only when there is a significant change in the vehicle's position. The system evaluates the minimum lane width necessary to accommodate the vehicle's turning radius and other dynamic factors. It then calculates the optimal expansion of the drivable area's boundaries to ensure there is adequate space for safe maneuvering, taking into account the vehicle's path curvature. The rate at which these boundaries can expand or contract is moderated to maintain stability in the vehicle's navigation. The algorithm aims to maximize the drivable space while avoiding fixed obstacles and adhering to legal driving limits. Finally, it applies these boundary adjustments and smooths out the path curvature calculations to ensure a safe and legally compliant navigable path is maintained throughout the vehicle's operation. + +!!! note + + The feature can be enabled in the [drivable_area_expansion.param.yaml](https://github.com/autowarefoundation/autoware_launch/blob/0cd5d891a36ac34a32a417205905c109f2bafe7b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml#L10). + +## Generating Turn Signal + +The Behavior Path Planner module uses the `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` to output turn signal commands (see [TurnIndicatorsCommand.idl](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg)). The system evaluates the driving context and determines when to activate turn signals based on its maneuver planning—like turning, lane changing, or obstacle avoidance. + +Within this framework, the system differentiates between **desired** and **required** blinker activations. **Desired** activations are those recommended by traffic laws for typical driving scenarios, such as signaling before a lane change or turn. **Required** activations are those that are deemed mandatory for safety reasons, like signaling an abrupt lane change to avoid an obstacle. + +The `TurnIndicatorsCommand` message structure has a command field that can take one of several constants: `NO_COMMAND` indicates no signal is necessary, `DISABLE` to deactivate signals, `ENABLE_LEFT` to signal a left turn, and `ENABLE_RIGHT` to signal a right turn. The Behavior Path Planner sends these commands at the appropriate times, based on its rules-based system that considers both the **desired** and **required** scenarios for blinker activation. + +!!! note + + For more in-depth information, refer to [Turn Signal Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md) document. + +## Rerouting + +!!! warning + + Rerouting is a feature that was still under progress. Further information will be included on a later date. + +## Parameters and Configuration + +The [configuration files](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner) are organized in a hierarchical directory structure for ease of navigation and management. Each subdirectory contains specific configuration files relevant to its module. The root directory holds general configuration files that apply to the overall behavior of the planner. The following is an overview of the directory structure with the respective configuration files. + +```text +behavior_path_planner +├── behavior_path_planner.param.yaml +├── drivable_area_expansion.param.yaml +├── scene_module_manager.param.yaml +├── static_obstacle_avoidance +│ └── static_obstacle_avoidance.param.yaml +├── avoidance_by_lc +│ └── avoidance_by_lc.param.yaml +├── dynamic_obstacle_avoidance +│ └── dynamic_obstacle_avoidance.param.yaml +├── goal_planner +│ └── goal_planner.param.yaml +├── lane_change +│ └── lane_change.param.yaml +├── side_shift +│ └── side_shift.param.yaml +└── start_planner + └── start_planner.param.yaml +``` + +Similarly, the [common](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/common) directory contains configuration files that are used across various modules, providing shared parameters and settings essential for the functioning of the Behavior Path Planner: + +```text +common +├── common.param.yaml +├── costmap_generator.param.yaml +└── nearest_search.param.yaml +``` + +The [preset](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/preset) directory contains the configurations for managing the operational state of various modules. It includes the default_preset.yaml file, which specifically caters to enabling and disabling modules within the system. + +```text +preset +└── default_preset.yaml +``` + +## Limitations & Future Work + +1. Goal Planner module cannot be simultaneously executed together with other modules. +2. Module is not designed as plugin. Integrating custom module is not straightforward and user have to modify some part of the behavior path planner main code. diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/behavior_path_planner.param.yaml rename to planning/autoware_behavior_path_planner/config/behavior_path_planner.param.yaml diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/drivable_area_expansion.param.yaml rename to planning/autoware_behavior_path_planner/config/drivable_area_expansion.param.yaml diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/autoware_behavior_path_planner/config/scene_module_manager.param.yaml similarity index 100% rename from planning/behavior_path_planner/config/scene_module_manager.param.yaml rename to planning/autoware_behavior_path_planner/config/scene_module_manager.param.yaml diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md b/planning/autoware_behavior_path_planner/docs/behavior_path_planner_interface_design.md similarity index 100% rename from planning/behavior_path_planner/docs/behavior_path_planner_interface_design.md rename to planning/autoware_behavior_path_planner/docs/behavior_path_planner_interface_design.md diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_limitations.md b/planning/autoware_behavior_path_planner/docs/behavior_path_planner_limitations.md similarity index 100% rename from planning/behavior_path_planner/docs/behavior_path_planner_limitations.md rename to planning/autoware_behavior_path_planner/docs/behavior_path_planner_limitations.md diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md similarity index 100% rename from planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md rename to planning/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md diff --git a/planning/behavior_path_planner/image/checking_module_transition.png b/planning/autoware_behavior_path_planner/image/checking_module_transition.png similarity index 100% rename from planning/behavior_path_planner/image/checking_module_transition.png rename to planning/autoware_behavior_path_planner/image/checking_module_transition.png diff --git a/planning/behavior_path_planner/image/limitations/limitation-chattering_shifts.png b/planning/autoware_behavior_path_planner/image/limitations/limitation-chattering_shifts.png similarity index 100% rename from planning/behavior_path_planner/image/limitations/limitation-chattering_shifts.png rename to planning/autoware_behavior_path_planner/image/limitations/limitation-chattering_shifts.png diff --git a/planning/behavior_path_planner/image/limitations/limitation-corner.png b/planning/autoware_behavior_path_planner/image/limitations/limitation-corner.png similarity index 100% rename from planning/behavior_path_planner/image/limitations/limitation-corner.png rename to planning/autoware_behavior_path_planner/image/limitations/limitation-corner.png diff --git a/planning/behavior_path_planner/image/limitations/limitation-intersection.png b/planning/autoware_behavior_path_planner/image/limitations/limitation-intersection.png similarity index 100% rename from planning/behavior_path_planner/image/limitations/limitation-intersection.png rename to planning/autoware_behavior_path_planner/image/limitations/limitation-intersection.png diff --git a/planning/behavior_path_planner/image/limitations/limitation01-01.png b/planning/autoware_behavior_path_planner/image/limitations/limitation01-01.png similarity index 100% rename from planning/behavior_path_planner/image/limitations/limitation01-01.png rename to planning/autoware_behavior_path_planner/image/limitations/limitation01-01.png diff --git a/planning/behavior_path_planner/image/limitations/limitation01-02-not-work.png b/planning/autoware_behavior_path_planner/image/limitations/limitation01-02-not-work.png similarity index 100% rename from planning/behavior_path_planner/image/limitations/limitation01-02-not-work.png rename to planning/autoware_behavior_path_planner/image/limitations/limitation01-02-not-work.png diff --git a/planning/behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png b/planning/autoware_behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png similarity index 100% rename from planning/behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png rename to planning/autoware_behavior_path_planner/image/limitations/limitation01-03-not-equal-length.png diff --git a/planning/behavior_path_planner/image/limitations/limitation01-04-equal-length.png b/planning/autoware_behavior_path_planner/image/limitations/limitation01-04-equal-length.png similarity index 100% rename from planning/behavior_path_planner/image/limitations/limitation01-04-equal-length.png rename to planning/autoware_behavior_path_planner/image/limitations/limitation01-04-equal-length.png diff --git a/planning/behavior_path_planner/image/limitations/limitation01-04-not-equal.png b/planning/autoware_behavior_path_planner/image/limitations/limitation01-04-not-equal.png similarity index 100% rename from planning/behavior_path_planner/image/limitations/limitation01-04-not-equal.png rename to planning/autoware_behavior_path_planner/image/limitations/limitation01-04-not-equal.png diff --git a/planning/behavior_path_planner/image/manager/approved_modules_stack.svg b/planning/autoware_behavior_path_planner/image/manager/approved_modules_stack.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/approved_modules_stack.svg rename to planning/autoware_behavior_path_planner/image/manager/approved_modules_stack.svg diff --git a/planning/behavior_path_planner/image/manager/avoidance.svg b/planning/autoware_behavior_path_planner/image/manager/avoidance.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/avoidance.svg rename to planning/autoware_behavior_path_planner/image/manager/avoidance.svg diff --git a/planning/behavior_path_planner/image/manager/candidate_modules_stack.svg b/planning/autoware_behavior_path_planner/image/manager/candidate_modules_stack.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/candidate_modules_stack.svg rename to planning/autoware_behavior_path_planner/image/manager/candidate_modules_stack.svg diff --git a/planning/behavior_path_planner/image/manager/current_route_lanelet.svg b/planning/autoware_behavior_path_planner/image/manager/current_route_lanelet.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/current_route_lanelet.svg rename to planning/autoware_behavior_path_planner/image/manager/current_route_lanelet.svg diff --git a/planning/behavior_path_planner/image/manager/example_behavior.svg b/planning/autoware_behavior_path_planner/image/manager/example_behavior.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/example_behavior.svg rename to planning/autoware_behavior_path_planner/image/manager/example_behavior.svg diff --git a/planning/behavior_path_planner/image/manager/failure_modules.svg b/planning/autoware_behavior_path_planner/image/manager/failure_modules.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/failure_modules.svg rename to planning/autoware_behavior_path_planner/image/manager/failure_modules.svg diff --git a/planning/behavior_path_planner/image/manager/lane_change.svg b/planning/autoware_behavior_path_planner/image/manager/lane_change.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/lane_change.svg rename to planning/autoware_behavior_path_planner/image/manager/lane_change.svg diff --git a/planning/behavior_path_planner/image/manager/lane_change_remove.svg b/planning/autoware_behavior_path_planner/image/manager/lane_change_remove.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/lane_change_remove.svg rename to planning/autoware_behavior_path_planner/image/manager/lane_change_remove.svg diff --git a/planning/behavior_path_planner/image/manager/lane_change_skip.svg b/planning/autoware_behavior_path_planner/image/manager/lane_change_skip.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/lane_change_skip.svg rename to planning/autoware_behavior_path_planner/image/manager/lane_change_skip.svg diff --git a/planning/behavior_path_planner/image/manager/manager_overview.svg b/planning/autoware_behavior_path_planner/image/manager/manager_overview.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/manager_overview.svg rename to planning/autoware_behavior_path_planner/image/manager/manager_overview.svg diff --git a/planning/behavior_path_planner/image/manager/module_select.svg b/planning/autoware_behavior_path_planner/image/manager/module_select.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/module_select.svg rename to planning/autoware_behavior_path_planner/image/manager/module_select.svg diff --git a/planning/behavior_path_planner/image/manager/multiple_candidates.svg b/planning/autoware_behavior_path_planner/image/manager/multiple_candidates.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/multiple_candidates.svg rename to planning/autoware_behavior_path_planner/image/manager/multiple_candidates.svg diff --git a/planning/behavior_path_planner/image/manager/output_module.svg b/planning/autoware_behavior_path_planner/image/manager/output_module.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/output_module.svg rename to planning/autoware_behavior_path_planner/image/manager/output_module.svg diff --git a/planning/behavior_path_planner/image/manager/process_step1.svg b/planning/autoware_behavior_path_planner/image/manager/process_step1.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/process_step1.svg rename to planning/autoware_behavior_path_planner/image/manager/process_step1.svg diff --git a/planning/behavior_path_planner/image/manager/process_step2.svg b/planning/autoware_behavior_path_planner/image/manager/process_step2.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/process_step2.svg rename to planning/autoware_behavior_path_planner/image/manager/process_step2.svg diff --git a/planning/behavior_path_planner/image/manager/process_step4.svg b/planning/autoware_behavior_path_planner/image/manager/process_step4.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/process_step4.svg rename to planning/autoware_behavior_path_planner/image/manager/process_step4.svg diff --git a/planning/behavior_path_planner/image/manager/process_step5.svg b/planning/autoware_behavior_path_planner/image/manager/process_step5.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/process_step5.svg rename to planning/autoware_behavior_path_planner/image/manager/process_step5.svg diff --git a/planning/behavior_path_planner/image/manager/process_step6.svg b/planning/autoware_behavior_path_planner/image/manager/process_step6.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/process_step6.svg rename to planning/autoware_behavior_path_planner/image/manager/process_step6.svg diff --git a/planning/behavior_path_planner/image/manager/request_step1.svg b/planning/autoware_behavior_path_planner/image/manager/request_step1.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/request_step1.svg rename to planning/autoware_behavior_path_planner/image/manager/request_step1.svg diff --git a/planning/behavior_path_planner/image/manager/request_step2-2.svg b/planning/autoware_behavior_path_planner/image/manager/request_step2-2.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/request_step2-2.svg rename to planning/autoware_behavior_path_planner/image/manager/request_step2-2.svg diff --git a/planning/behavior_path_planner/image/manager/request_step2-3.svg b/planning/autoware_behavior_path_planner/image/manager/request_step2-3.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/request_step2-3.svg rename to planning/autoware_behavior_path_planner/image/manager/request_step2-3.svg diff --git a/planning/behavior_path_planner/image/manager/request_step2.svg b/planning/autoware_behavior_path_planner/image/manager/request_step2.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/request_step2.svg rename to planning/autoware_behavior_path_planner/image/manager/request_step2.svg diff --git a/planning/behavior_path_planner/image/manager/request_step3.svg b/planning/autoware_behavior_path_planner/image/manager/request_step3.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/request_step3.svg rename to planning/autoware_behavior_path_planner/image/manager/request_step3.svg diff --git a/planning/behavior_path_planner/image/manager/request_step4-2.svg b/planning/autoware_behavior_path_planner/image/manager/request_step4-2.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/request_step4-2.svg rename to planning/autoware_behavior_path_planner/image/manager/request_step4-2.svg diff --git a/planning/behavior_path_planner/image/manager/request_step4-3.svg b/planning/autoware_behavior_path_planner/image/manager/request_step4-3.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/request_step4-3.svg rename to planning/autoware_behavior_path_planner/image/manager/request_step4-3.svg diff --git a/planning/behavior_path_planner/image/manager/request_step4.svg b/planning/autoware_behavior_path_planner/image/manager/request_step4.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/request_step4.svg rename to planning/autoware_behavior_path_planner/image/manager/request_step4.svg diff --git a/planning/behavior_path_planner/image/manager/request_step5.svg b/planning/autoware_behavior_path_planner/image/manager/request_step5.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/request_step5.svg rename to planning/autoware_behavior_path_planner/image/manager/request_step5.svg diff --git a/planning/behavior_path_planner/image/manager/root_generation.svg b/planning/autoware_behavior_path_planner/image/manager/root_generation.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/root_generation.svg rename to planning/autoware_behavior_path_planner/image/manager/root_generation.svg diff --git a/planning/behavior_path_planner/image/manager/scene_module.svg b/planning/autoware_behavior_path_planner/image/manager/scene_module.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/scene_module.svg rename to planning/autoware_behavior_path_planner/image/manager/scene_module.svg diff --git a/planning/behavior_path_planner/image/manager/sub_managers.svg b/planning/autoware_behavior_path_planner/image/manager/sub_managers.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/sub_managers.svg rename to planning/autoware_behavior_path_planner/image/manager/sub_managers.svg diff --git a/planning/behavior_path_planner/image/manager/success_modules_remove.svg b/planning/autoware_behavior_path_planner/image/manager/success_modules_remove.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/success_modules_remove.svg rename to planning/autoware_behavior_path_planner/image/manager/success_modules_remove.svg diff --git a/planning/behavior_path_planner/image/manager/success_modules_skip.svg b/planning/autoware_behavior_path_planner/image/manager/success_modules_skip.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/success_modules_skip.svg rename to planning/autoware_behavior_path_planner/image/manager/success_modules_skip.svg diff --git a/planning/behavior_path_planner/image/manager/waiting_approve.svg b/planning/autoware_behavior_path_planner/image/manager/waiting_approve.svg similarity index 100% rename from planning/behavior_path_planner/image/manager/waiting_approve.svg rename to planning/autoware_behavior_path_planner/image/manager/waiting_approve.svg diff --git a/planning/behavior_path_planner/image/static_drivable_area_after_expansion.png b/planning/autoware_behavior_path_planner/image/static_drivable_area_after_expansion.png similarity index 100% rename from planning/behavior_path_planner/image/static_drivable_area_after_expansion.png rename to planning/autoware_behavior_path_planner/image/static_drivable_area_after_expansion.png diff --git a/planning/behavior_path_planner/image/static_drivable_area_before_expansion.png b/planning/autoware_behavior_path_planner/image/static_drivable_area_before_expansion.png similarity index 100% rename from planning/behavior_path_planner/image/static_drivable_area_before_expansion.png rename to planning/autoware_behavior_path_planner/image/static_drivable_area_before_expansion.png diff --git a/planning/behavior_path_planner/image/supported_module_avoidance.svg b/planning/autoware_behavior_path_planner/image/supported_module_avoidance.svg similarity index 100% rename from planning/behavior_path_planner/image/supported_module_avoidance.svg rename to planning/autoware_behavior_path_planner/image/supported_module_avoidance.svg diff --git a/planning/behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg b/planning/autoware_behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg similarity index 100% rename from planning/behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg rename to planning/autoware_behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg diff --git a/planning/behavior_path_planner/image/supported_module_goal_planner.svg b/planning/autoware_behavior_path_planner/image/supported_module_goal_planner.svg similarity index 100% rename from planning/behavior_path_planner/image/supported_module_goal_planner.svg rename to planning/autoware_behavior_path_planner/image/supported_module_goal_planner.svg diff --git a/planning/behavior_path_planner/image/supported_module_lane_change.svg b/planning/autoware_behavior_path_planner/image/supported_module_lane_change.svg similarity index 100% rename from planning/behavior_path_planner/image/supported_module_lane_change.svg rename to planning/autoware_behavior_path_planner/image/supported_module_lane_change.svg diff --git a/planning/behavior_path_planner/image/supported_module_lane_following.svg b/planning/autoware_behavior_path_planner/image/supported_module_lane_following.svg similarity index 100% rename from planning/behavior_path_planner/image/supported_module_lane_following.svg rename to planning/autoware_behavior_path_planner/image/supported_module_lane_following.svg diff --git a/planning/behavior_path_planner/image/supported_module_start_planner.svg b/planning/autoware_behavior_path_planner/image/supported_module_start_planner.svg similarity index 100% rename from planning/behavior_path_planner/image/supported_module_start_planner.svg rename to planning/autoware_behavior_path_planner/image/supported_module_start_planner.svg diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/behavior_path_planner_node.hpp similarity index 93% rename from planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp rename to planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/behavior_path_planner_node.hpp index 3be680aa06662..71e322cd4625d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/behavior_path_planner_node.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ -#define BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ -#include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/interface/steering_factor_interface.hpp" +#include "autoware_behavior_path_planner/planner_manager.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include @@ -49,7 +49,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_map_msgs::msg::LaneletMapBin; @@ -226,6 +226,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/planner_manager.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp rename to planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/planner_manager.hpp index 4eabe5f90d877..3e2afc2da6a95 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/autoware_behavior_path_planner/include/autoware_behavior_path_planner/planner_manager.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -36,7 +36,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using tier4_autoware_utils::StopWatch; @@ -469,6 +469,6 @@ class PlannerManager size_t max_iteration_num_{100}; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/autoware_behavior_path_planner/launch/behavior_path_planner.launch.xml similarity index 100% rename from planning/behavior_path_planner/launch/behavior_path_planner.launch.xml rename to planning/autoware_behavior_path_planner/launch/behavior_path_planner.launch.xml diff --git a/planning/autoware_behavior_path_planner/package.xml b/planning/autoware_behavior_path_planner/package.xml new file mode 100644 index 0000000000000..2efc32531432b --- /dev/null +++ b/planning/autoware_behavior_path_planner/package.xml @@ -0,0 +1,79 @@ + + + + autoware_behavior_path_planner + 0.1.0 + The behavior_path_planner package + + + Zulfaqar Azmi + + Fumiya Watanabe + Kyoichi Sugahara + + + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + Takamasa Horibe + Satoshi Ota + Kosuke Takeuchi + Kyoichi Sugahara + Takayuki Murooka + Go Sakayori + + Apache License 2.0 + + Takamasa Horibe + Satoshi Ota + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Yutaka Shimizu + Takayuki Murooka + Ryohsuke Mitsudome + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_behavior_path_planner_common + autoware_freespace_planning_algorithms + autoware_frenet_planner + autoware_lane_departure_checker + autoware_path_sampler + autoware_perception_msgs + autoware_planning_msgs + autoware_planning_test_manager + autoware_vehicle_info_utils + autoware_vehicle_msgs + behaviortree_cpp_v3 + geometry_msgs + interpolation + lanelet2_extension + libboost-dev + libopencv-dev + magic_enum + motion_utils + object_recognition_utils + pluginlib + rclcpp + rclcpp_components + sensor_msgs + signal_processing + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp similarity index 98% rename from planning/behavior_path_planner/src/behavior_path_planner_node.cpp rename to planning/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index f50cb94f436e6..d9478ab4c1e93 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "motion_utils/trajectory/conversion.hpp" +#include #include -#include #include @@ -42,10 +42,10 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) } } // namespace -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using autoware::vehicle_info_utils::VehicleInfoUtils; using tier4_planning_msgs::msg::PathChangeModuleId; -using vehicle_info_util::VehicleInfoUtil; BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options) : Node("behavior_path_planner", node_options) @@ -212,7 +212,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); // vehicle info - const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = VehicleInfoUtils(*this).getVehicleInfo(); p.vehicle_info = vehicle_info; p.vehicle_width = vehicle_info.vehicle_width_m; p.vehicle_length = vehicle_info.vehicle_length_m; @@ -949,7 +949,7 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( return result; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #include -RCLCPP_COMPONENTS_REGISTER_NODE(behavior_path_planner::BehaviorPathPlannerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::behavior_path_planner::BehaviorPathPlannerNode) diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/autoware_behavior_path_planner/src/planner_manager.cpp similarity index 98% rename from planning/behavior_path_planner/src/planner_manager.cpp rename to planning/autoware_behavior_path_planner/src/planner_manager.cpp index 6cb2fccb31117..3f440615bdc2c 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_path_planner/src/planner_manager.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/planner_manager.hpp" +#include "autoware_behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -28,10 +28,12 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { PlannerManager::PlannerManager(rclcpp::Node & node, const size_t max_iteration_num) -: plugin_loader_("behavior_path_planner", "behavior_path_planner::SceneModuleManagerInterface"), +: plugin_loader_( + "autoware_behavior_path_planner", + "autoware::behavior_path_planner::SceneModuleManagerInterface"), logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), max_iteration_num_{max_iteration_num} @@ -979,4 +981,4 @@ std::string PlannerManager::getNames(const std::vector & modules return ss.str(); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/test/input.cpp b/planning/autoware_behavior_path_planner/test/input.cpp similarity index 96% rename from planning/behavior_path_planner/test/input.cpp rename to planning/autoware_behavior_path_planner/test/input.cpp index ba87af1525692..f4f368008c472 100644 --- a/planning/behavior_path_planner/test/input.cpp +++ b/planning/autoware_behavior_path_planner/test/input.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include "input.hpp" -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Twist; @@ -87,4 +87,4 @@ Pose generateEgoSamplePose(float && p_x, float && p_y, float && p_z) pose.position.z = p_z; return pose; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/test/input.hpp b/planning/autoware_behavior_path_planner/test/input.hpp similarity index 94% rename from planning/behavior_path_planner/test/input.hpp rename to planning/autoware_behavior_path_planner/test/input.hpp index 33e538156990b..f1617cdd9ead0 100644 --- a/planning/behavior_path_planner/test/input.hpp +++ b/planning/autoware_behavior_path_planner/test/input.hpp @@ -25,7 +25,7 @@ #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Pose; @@ -41,4 +41,4 @@ PathWithLaneId generateDiagonalSamplePathWithLaneId( Twist generateSampleEgoTwist(float && l_x, float && l_y, float && l_z); Pose generateEgoSamplePose(float && p_x, float && p_y, float && p_z); -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..5422bffa2094a --- /dev/null +++ b/planning/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp @@ -0,0 +1,122 @@ +// 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 "autoware_behavior_path_planner/behavior_path_planner_node.hpp" + +#include +#include +#include + +#include + +#include +#include + +using autoware::behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/behavior_path_planner/test/test_utils.cpp b/planning/autoware_behavior_path_planner/test/test_utils.cpp similarity index 84% rename from planning/behavior_path_planner/test/test_utils.cpp rename to planning/autoware_behavior_path_planner/test/test_utils.cpp index fb2f46cb8b9c0..cda78df2e8273 100644 --- a/planning/behavior_path_planner/test/test_utils.cpp +++ b/planning/autoware_behavior_path_planner/test/test_utils.cpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "input.hpp" #include "lanelet2_core/Attribute.h" #include "lanelet2_core/geometry/LineString.h" @@ -23,20 +23,20 @@ #include #include -using behavior_path_planner::PathWithLaneId; -using behavior_path_planner::Pose; -using behavior_path_planner::utils::FrenetPoint; +using autoware::behavior_path_planner::PathWithLaneId; +using autoware::behavior_path_planner::Pose; +using autoware::behavior_path_planner::utils::FrenetPoint; using geometry_msgs::msg::Point; TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnStraightLine) { PathWithLaneId path = - behavior_path_planner::generateStraightSamplePathWithLaneId(0.0f, 1.0f, 10u); - Pose vehicle_pose = behavior_path_planner::generateEgoSamplePose(10.7f, -1.7f, 0.0); + autoware::behavior_path_planner::generateStraightSamplePathWithLaneId(0.0f, 1.0f, 10u); + Pose vehicle_pose = autoware::behavior_path_planner::generateEgoSamplePose(10.7f, -1.7f, 0.0); const size_t vehicle_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, vehicle_pose, 3.0, 1.0); - const auto vehicle_pose_frenet = behavior_path_planner::utils::convertToFrenetPoint( + const auto vehicle_pose_frenet = autoware::behavior_path_planner::utils::convertToFrenetPoint( path.points, vehicle_pose.position, vehicle_seg_idx); EXPECT_NEAR(vehicle_pose_frenet.distance, -1.7f, 1e-3); @@ -46,12 +46,12 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnStraightLin TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnDiagonalLine) { PathWithLaneId path = - behavior_path_planner::generateDiagonalSamplePathWithLaneId(0.0f, 1.0f, 10u); - Pose vehicle_pose = behavior_path_planner::generateEgoSamplePose(0.1f, 0.1f, 0.0); + autoware::behavior_path_planner::generateDiagonalSamplePathWithLaneId(0.0f, 1.0f, 10u); + Pose vehicle_pose = autoware::behavior_path_planner::generateEgoSamplePose(0.1f, 0.1f, 0.0); const size_t vehicle_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, vehicle_pose, 3.0, 1.0); - const auto vehicle_pose_frenet = behavior_path_planner::utils::convertToFrenetPoint( + const auto vehicle_pose_frenet = autoware::behavior_path_planner::utils::convertToFrenetPoint( path.points, vehicle_pose.position, vehicle_seg_idx); EXPECT_NEAR(vehicle_pose_frenet.distance, 0, 1e-2); @@ -60,7 +60,8 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, vehiclePoseToFrenetOnDiagonalLin TEST(BehaviorPathPlanningUtilitiesBehaviorTest, setGoal) { - PathWithLaneId path = behavior_path_planner::generateDiagonalSamplePathWithLaneId(0.0f, 1.0f, 5u); + PathWithLaneId path = + autoware::behavior_path_planner::generateDiagonalSamplePathWithLaneId(0.0f, 1.0f, 5u); path.points.at(0).lane_ids.push_back(0); path.points.at(1).lane_ids.push_back(1); path.points.at(2).lane_ids.push_back(2); @@ -70,7 +71,7 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, setGoal) path.points.at(4).lane_ids.push_back(5); PathWithLaneId path_with_goal; - behavior_path_planner::utils::setGoal( + autoware::behavior_path_planner::utils::setGoal( 3.5, M_PI * 0.5, path, path.points.back().point.pose, 5, &path_with_goal); // Check if skipped lane ids by smooth skip connection are filled in output path. @@ -83,8 +84,8 @@ TEST(BehaviorPathPlanningUtilitiesBehaviorTest, setGoal) TEST(BehaviorPathPlanningUtilitiesBehaviorTest, expandLanelets) { - using behavior_path_planner::DrivableLanes; - using behavior_path_planner::utils::expandLanelets; + using autoware::behavior_path_planner::DrivableLanes; + using autoware::behavior_path_planner::utils::expandLanelets; std::vector original_lanelets; { // empty list of lanelets, empty output const auto expanded_lanelets = expandLanelets(original_lanelets, 0.0, 0.0); diff --git a/planning/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/autoware_behavior_path_planner_common/CMakeLists.txt new file mode 100644 index 0000000000000..d3774bb004826 --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_path_planner_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(magic_enum CONFIG REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/turn_signal_decider.cpp + src/interface/steering_factor_interface.cpp + src/interface/scene_module_visitor.cpp + src/utils/utils.cpp + src/utils/path_utils.cpp + src/utils/traffic_light_utils.cpp + src/utils/path_safety_checker/safety_check.cpp + src/utils/path_safety_checker/objects_filtering.cpp + src/utils/path_shifter/path_shifter.cpp + src/utils/drivable_area_expansion/static_drivable_area.cpp + 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/parking_departure/geometric_parallel_parking.cpp + src/utils/parking_departure/utils.cpp + src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp + src/marker_utils/utils.cpp +) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + ${EIGEN3_INCLUDE_DIR} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities + test/test_drivable_area_expansion.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_utilities + ${PROJECT_NAME} + ) + + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_safety_check + test/test_safety_check.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_safety_check + ${PROJECT_NAME} + ) + + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_turn_signal + test/test_turn_signal.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_turn_signal + ${PROJECT_NAME} + ) +endif() + +ament_auto_package() diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md b/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md similarity index 100% rename from planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md rename to planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md b/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md similarity index 97% rename from planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md rename to planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md index 2a22db5597241..dd2e4df00bddd 100644 --- a/planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md +++ b/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md @@ -1,6 +1,6 @@ # Path Generation design -This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp). +This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp). ## Overview diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check.md b/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md similarity index 100% rename from planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check.md rename to planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md b/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md similarity index 100% rename from planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md rename to planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png b/planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png rename to planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png diff --git a/planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png b/planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png rename to planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png diff --git a/planning/behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png b/planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png rename to planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png diff --git a/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png b/planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png rename to planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png diff --git a/planning/behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg diff --git a/planning/behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg b/planning/autoware_behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg diff --git a/planning/behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg b/planning/autoware_behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg diff --git a/planning/behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg b/planning/autoware_behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg diff --git a/planning/behavior_path_planner_common/images/path_shifter/path_shifter.png b/planning/autoware_behavior_path_planner_common/images/path_shifter/path_shifter.png similarity index 100% rename from planning/behavior_path_planner_common/images/path_shifter/path_shifter.png rename to planning/autoware_behavior_path_planner_common/images/path_shifter/path_shifter.png diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/data_manager.hpp similarity index 94% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/data_manager.hpp index 90a2bf58a4e13..9f0c73df72117 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/data_manager.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ -#include "behavior_path_planner_common/parameters.hpp" -#include "behavior_path_planner_common/turn_signal_decider.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/parameters.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include #include #include #include #include -#include #include #include @@ -48,8 +48,9 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using autoware::route_handler::RouteHandler; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; @@ -61,7 +62,6 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; -using route_handler::RouteHandler; using tier4_planning_msgs::msg::LateralOffset; using tier4_planning_msgs::msg::PathWithLaneId; using PlanResult = PathWithLaneId::SharedPtr; @@ -267,6 +267,6 @@ struct PlannerData } }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp new file mode 100644 index 0000000000000..a9af21a34cab3 --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp @@ -0,0 +1,648 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ + +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.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 + +namespace autoware::behavior_path_planner +{ +using autoware::objects_of_interest_marker_interface::ColorName; +using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using autoware::rtc_interface::RTCInterface; +using autoware_adapi_v1_msgs::msg::PlanningBehavior; +using autoware_adapi_v1_msgs::msg::SteeringFactor; +using steering_factor_interface::SteeringFactorInterface; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::generateUUID; +using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; +using tier4_rtc_msgs::msg::State; +using unique_identifier_msgs::msg::UUID; +using visualization_msgs::msg::MarkerArray; +using PlanResult = PathWithLaneId::SharedPtr; + +enum class ModuleStatus { + IDLE = 0, + RUNNING = 1, + WAITING_APPROVAL = 2, + SUCCESS = 3, + FAILURE = 4, +}; + +class SceneModuleInterface +{ +public: + SceneModuleInterface( + const std::string & name, rclcpp::Node & node, + std::unordered_map> rtc_interface_ptr_map, + std::unordered_map> + objects_of_interest_marker_interface_ptr_map) + : name_{name}, + logger_{node.get_logger().get_child(name)}, + clock_{node.get_clock()}, + rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), + objects_of_interest_marker_interface_ptr_map_( + std::move(objects_of_interest_marker_interface_ptr_map)), + steering_factor_interface_ptr_( + std::make_unique(&node, utils::convertToSnakeCase(name))) + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + uuid_map_.emplace(module_name, generateUUID()); + } + } + + SceneModuleInterface(const SceneModuleInterface &) = delete; + SceneModuleInterface(SceneModuleInterface &&) = delete; + SceneModuleInterface & operator=(const SceneModuleInterface &) = delete; + SceneModuleInterface & operator=(SceneModuleInterface &&) = delete; + virtual ~SceneModuleInterface() = default; + + virtual void updateModuleParams(const std::any & parameters) = 0; + + virtual void acceptVisitor(const std::shared_ptr & visitor) const = 0; + + /** + * @brief Return true if the module has request for execution (not necessarily feasible) + */ + virtual bool isExecutionRequested() const = 0; + + /** + * @brief Return true if the execution is available (e.g. safety check for lane change) + */ + virtual bool isExecutionReady() const = 0; + + /** + * @brief update data for planning. Note that the call of this function does not mean + * that the module executed. It should only updates the data necessary for + * planCandidate (e.g., resampling of path). + */ + 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. + */ + virtual BehaviorModuleOutput run() + { + updateData(); + const auto output = isWaitingApproval() ? planWaitingApproval() : plan(); + try { + motion_utils::validateNonEmpty(output.path.points); + } catch (const std::exception & ex) { + throw std::invalid_argument("[" + name_ + "]" + ex.what()); + } + return output; + } + + /** + * @brief Set the current_state_ based on updateState output. + */ + void updateCurrentState() + { + const auto print = [this](const auto & from, const auto & to) { + RCLCPP_DEBUG( + getLogger(), "[%s] Transit from %s to %s.", name_.c_str(), from.data(), to.data()); + }; + + const auto & from = current_state_; + current_state_ = updateState(); + print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); + } + + /** + * @brief Called on the first time when the module goes into RUNNING. + */ + void onEntry() + { + RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); + + stop_reason_ = StopReason(); + + processOnEntry(); + } + + /** + * @brief Called when the module exit from RUNNING. + */ + void onExit() + { + RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); + + if (getCurrentStatus() == ModuleStatus::SUCCESS) { + updateRTCStatusForSuccess(); + } + + clearWaitingApproval(); + unlockNewModuleLaunch(); + unlockOutputPath(); + steering_factor_interface_ptr_->clearSteeringFactors(); + + stop_reason_ = StopReason(); + + processOnExit(); + } + + void publishObjectsOfInterestMarker() + { + for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) { + if (ptr) { + ptr->publishMarkerArray(); + } + } + } + + void publishSteeringFactor() + { + if (!steering_factor_interface_ptr_) { + return; + } + steering_factor_interface_ptr_->publishSteeringFactor(clock_->now()); + } + + void lockRTCCommand() + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->lockCommandUpdate(); + } + } + } + + void unlockRTCCommand() + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->unlockCommandUpdate(); + } + } + } + + /** + * @brief set previous module's output as input for this module + */ + void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + { + previous_module_output_ = previous_module_output; + } + + /** + * @brief set planner data + */ + virtual void setData(const std::shared_ptr & data) { planner_data_ = data; } + + void lockOutputPath() { is_locked_output_path_ = true; } + + void unlockOutputPath() { is_locked_output_path_ = false; } + + bool isWaitingApproval() const + { + return is_waiting_approval_ || current_state_ == ModuleStatus::WAITING_APPROVAL; + } + + virtual bool isCurrentRouteLaneletToBeReset() const { return false; } + + bool isLockedNewModuleLaunch() const { return is_locked_new_module_launch_; } + + PlanResult getPathCandidate() const { return path_candidate_; } + + PlanResult getPathReference() const { return path_reference_; } + + MarkerArray getInfoMarkers() const { return info_marker_; } + + MarkerArray getDebugMarkers() const { return debug_marker_; } + + MarkerArray getDrivableLanesMarkers() const { return drivable_lanes_marker_; } + + virtual MarkerArray getModuleVirtualWall() { return MarkerArray(); } + + ModuleStatus getCurrentStatus() const { return current_state_; } + + StopReason getStopReason() const { return stop_reason_; } + + std::string name() const { return name_; } + + std::optional getStopPose() const + { + if (!stop_pose_) { + return {}; + } + + const auto & base_link2front = planner_data_->parameters.base_link2front; + return calcOffsetPose(stop_pose_.value(), base_link2front, 0.0, 0.0); + } + + std::optional getSlowPose() const + { + if (!slow_pose_) { + return {}; + } + + const auto & base_link2front = planner_data_->parameters.base_link2front; + return calcOffsetPose(slow_pose_.value(), base_link2front, 0.0, 0.0); + } + + std::optional getDeadPose() const + { + if (!dead_pose_) { + return {}; + } + + const auto & base_link2front = planner_data_->parameters.base_link2front; + return calcOffsetPose(dead_pose_.value(), base_link2front, 0.0, 0.0); + } + + void resetWallPoses() const + { + stop_pose_ = std::nullopt; + slow_pose_ = std::nullopt; + dead_pose_ = std::nullopt; + } + + rclcpp::Logger getLogger() const { return logger_; } + +private: + bool existRegisteredRequest() const + { + return std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), + [&](const auto & rtc) { return rtc.second->isRegistered(uuid_map_.at(rtc.first)); }); + } + + bool existApprovedRequest() const + { + return std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { + return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && + rtc.second->isActivated(uuid_map_.at(rtc.first)); + }); + } + + bool existNotApprovedRequest() const + { + return std::any_of( + rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { + return rtc.second->isRegistered(uuid_map_.at(rtc.first)) && + !rtc.second->isActivated(uuid_map_.at(rtc.first)); + }); + } + + bool canTransitWaitingApprovalState() const + { + if (!existRegisteredRequest()) { + return false; + } + return existNotApprovedRequest(); + } + + bool canTransitWaitingApprovalToRunningState() const + { + if (!existRegisteredRequest()) { + return true; + } + return existApprovedRequest(); + } + + /** + * @brief Return SUCCESS if plan is not needed or plan is successfully finished, + * FAILURE if plan has failed, RUNNING if plan is on going. + * These condition is to be implemented in each modules. + */ + ModuleStatus updateState() + { + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + if (current_state_ == ModuleStatus::IDLE) { + auto init_state = setInitState(); + RCLCPP_DEBUG( + getLogger(), "transiting from IDLE to %s", magic_enum::enum_name(init_state).data()); + return init_state; + } + + if (current_state_ == ModuleStatus::RUNNING) { + if (canTransitSuccessState()) { + log_debug_throttled("transiting from RUNNING to SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (canTransitFailureState()) { + log_debug_throttled("transiting from RUNNING to FAILURE"); + return ModuleStatus::FAILURE; + } + + if (canTransitWaitingApprovalState()) { + log_debug_throttled("transiting from RUNNING to WAITING_APPROVAL"); + return ModuleStatus::WAITING_APPROVAL; + } + + log_debug_throttled("transiting from RUNNING to RUNNING"); + return ModuleStatus::RUNNING; + } + + if (current_state_ == ModuleStatus::WAITING_APPROVAL) { + if (canTransitSuccessState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (canTransitFailureState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to FAILURE"); + return ModuleStatus::FAILURE; + } + + if (canTransitWaitingApprovalToRunningState()) { + log_debug_throttled("transiting from WAITING_APPROVAL to RUNNING"); + return ModuleStatus::RUNNING; + } + + log_debug_throttled("transiting from WAITING_APPROVAL to WAITING APPROVAL"); + return ModuleStatus::WAITING_APPROVAL; + } + + if (current_state_ == ModuleStatus::SUCCESS) { + log_debug_throttled("already SUCCESS"); + return ModuleStatus::SUCCESS; + } + + if (current_state_ == ModuleStatus::FAILURE) { + log_debug_throttled("already FAILURE"); + return ModuleStatus::FAILURE; + } + + log_debug_throttled("already IDLE"); + return ModuleStatus::IDLE; + } + + std::string name_; + + ModuleStatus current_state_{ModuleStatus::IDLE}; + + BehaviorModuleOutput previous_module_output_; + + StopReason stop_reason_; + + bool is_locked_new_module_launch_{false}; + + bool is_locked_output_path_{false}; + +protected: + /** + * @brief State transition condition ANY -> SUCCESS + */ + virtual bool canTransitSuccessState() = 0; + + /** + * @brief State transition condition ANY -> FAILURE + */ + virtual bool canTransitFailureState() = 0; + + /** + * @brief Explicitly set the initial state + */ + virtual ModuleStatus setInitState() const { return ModuleStatus::RUNNING; } + + /** + * @brief Get candidate path. This information is used for external judgement. + */ + virtual CandidateOutput planCandidate() const = 0; + + /** + * @brief Calculate path. This function is called with the plan is approved. + */ + virtual BehaviorModuleOutput plan() = 0; + + /** + * @brief Calculate path under waiting_approval condition. + * The default implementation is just to return the reference path. + */ + virtual BehaviorModuleOutput planWaitingApproval() + { + path_candidate_ = std::make_shared(planCandidate().path_candidate); + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); + + return getPreviousModuleOutput(); + } + + /** + * @brief Module unique entry process. + */ + virtual void processOnEntry() {} + + /** + * @brief Module unique exit process. + */ + virtual void processOnExit() {} + + virtual void updateRTCStatus(const double start_distance, const double finish_distance) + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + const auto state = isWaitingApproval() ? State::WAITING_FOR_EXECUTION : State::RUNNING; + ptr->updateCooperateStatus( + uuid_map_.at(module_name), isExecutionReady(), state, start_distance, finish_distance, + clock_->now()); + } + } + } + + void updateRTCStatusForSuccess() + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + if (ptr->isRegistered(uuid_map_.at(module_name))) { + ptr->updateCooperateStatus( + uuid_map_.at(module_name), true, State::SUCCEEDED, + std::numeric_limits::lowest(), std::numeric_limits::lowest(), + clock_->now()); + } + } + } + } + + void setObjectsOfInterestData( + const geometry_msgs::msg::Pose & obj_pose, + const autoware_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) + { + for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) { + if (ptr) { + ptr->insertObjectData(obj_pose, obj_shape, color_name); + } + } + } + + /** + * @brief Return true if the activation command is received from the RTC interface. + * If no RTC interface is registered, return true. + */ + bool isActivated() const + { + if (rtc_interface_ptr_map_.empty()) { + return true; + } + + if (!existRegisteredRequest()) { + return false; + } + return existApprovedRequest(); + } + + void removeRTCStatus() + { + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + if (ptr) { + ptr->clearCooperateStatus(); + } + } + } + + void setStopReason(const std::string & stop_reason, const PathWithLaneId & path) + { + stop_reason_.reason = stop_reason; + stop_reason_.stop_factors.clear(); + + if (!stop_pose_) { + stop_reason_.reason = ""; + return; + } + + StopFactor stop_factor; + stop_factor.stop_pose = stop_pose_.value(); + stop_factor.dist_to_stop_pose = + motion_utils::calcSignedArcLength(path.points, getEgoPosition(), stop_pose_.value().position); + stop_reason_.stop_factors.push_back(stop_factor); + } + + void setDrivableLanes(const std::vector & drivable_lanes) + { + drivable_lanes_marker_ = + marker_utils::createDrivableLanesMarkerArray(drivable_lanes, "drivable_lanes"); + } + + BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } + + bool isOutputPathLocked() const { return is_locked_output_path_; } + + void lockNewModuleLaunch() { is_locked_new_module_launch_ = true; } + + void unlockNewModuleLaunch() { is_locked_new_module_launch_ = false; } + + void waitApproval() { is_waiting_approval_ = true; } + + void clearWaitingApproval() { is_waiting_approval_ = false; } + + void resetPathCandidate() { path_candidate_.reset(); } + + void resetPathReference() { path_reference_.reset(); } + + geometry_msgs::msg::Point getEgoPosition() const + { + return planner_data_->self_odometry->pose.pose.position; + } + + geometry_msgs::msg::Pose getEgoPose() const { return planner_data_->self_odometry->pose.pose; } + + geometry_msgs::msg::Twist getEgoTwist() const + { + return planner_data_->self_odometry->twist.twist; + } + + double getEgoSpeed() const + { + return std::abs(planner_data_->self_odometry->twist.twist.linear.x); + } + + rclcpp::Logger logger_; + + rclcpp::Clock::SharedPtr clock_; + + std::shared_ptr planner_data_; + + bool is_waiting_approval_{false}; + + std::unordered_map uuid_map_; + + PlanResult path_candidate_; + PlanResult path_reference_; + + std::unordered_map> rtc_interface_ptr_map_; + + std::unordered_map> + objects_of_interest_marker_interface_ptr_map_; + + std::unique_ptr steering_factor_interface_ptr_; + + mutable std::optional stop_pose_{std::nullopt}; + + mutable std::optional slow_pose_{std::nullopt}; + + mutable std::optional dead_pose_{std::nullopt}; + + mutable MarkerArray info_marker_; + + mutable MarkerArray debug_marker_; + + mutable MarkerArray drivable_lanes_marker_; +}; + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp similarity index 96% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 2eb56104ec831..bc4505710a6cc 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include @@ -33,7 +33,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using motion_utils::createDeadLineVirtualWallMarker; @@ -346,6 +346,6 @@ class SceneModuleManagerInterface ModuleConfigParameters config_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp similarity index 79% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp index 1bce30b18edd7..384c74243fcb2 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { // Forward Declaration class StaticObstacleAvoidanceModule; @@ -42,6 +42,6 @@ class SceneModuleVisitor protected: mutable std::shared_ptr avoidance_visitor_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp similarity index 85% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp index fb0cb97a3db95..31f169471cc9e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ #include @@ -52,4 +52,4 @@ class SteeringFactorInterface } // namespace steering_factor_interface -#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/colors.hpp similarity index 91% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/colors.hpp index 1c066ff744f46..ebc48f9c16cc8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/colors.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ #include "tier4_autoware_utils/ros/marker_helper.hpp" @@ -91,4 +91,4 @@ inline std::vector colors_list(float a = 0.99) } } // namespace marker_utils::colors -#endif // BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp new file mode 100644 index 0000000000000..871820489dc9c --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp @@ -0,0 +1,125 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ + +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace marker_utils +{ +using autoware::behavior_path_planner::DrivableLanes; +using autoware::behavior_path_planner::ShiftLineArray; +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair; +using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Polygon; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using geometry_msgs::msg::Vector3; +using std_msgs::msg::ColorRGBA; +using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +inline int64_t bitShift(int64_t original_id) +{ + return original_id << (sizeof(int32_t) * 8 / 2); +} + +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b); + +MarkerArray createPoseMarkerArray( + const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, + const float & b); + +MarkerArray createPathMarkerArray( + const PathWithLaneId & path, std::string && ns, const int64_t & lane_id, const float & r, + const float & g, const float & b); + +MarkerArray createShiftLineMarkerArray( + const ShiftLineArray & shift_lines, const double & base_shift, std::string && ns, const float & r, + const float & g, const float & b, const float & w); + +MarkerArray createShiftLengthMarkerArray( + const std::vector & shift_distance, const PathWithLaneId & reference, std::string && ns, + const float & r, const float & g, const float & b); + +MarkerArray createShiftGradMarkerArray( + const std::vector & grad, const std::vector & shift_distance, + const PathWithLaneId & reference, std::string && ns, const float & r, const float & g, + const float & b); + +MarkerArray createLaneletsAreaMarkerArray( + const std::vector & lanelets, std::string && ns, const float & r, + const float & g, const float & b); + +MarkerArray createPolygonMarkerArray( + const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, + const float & g, const float & b, const float & w = 0.3); + +MarkerArray createObjectsMarkerArray( + const PredictedObjects & objects, std::string && ns, const int64_t & lane_id, const float & r, + const float & g, const float & b); + +MarkerArray createDrivableLanesMarkerArray( + const std::vector & drivable_lanes, std::string && ns); + +MarkerArray createPredictedPathMarkerArray( + const PredictedPath & ego_predicted_path, + const autoware::vehicle_info_utils::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 // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp new file mode 100644 index 0000000000000..2f14e227d05ef --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp @@ -0,0 +1,82 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ + +#include + +struct ModuleConfigParameters +{ + bool enable_module{false}; + 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}; +}; + +struct BehaviorPathPlannerParameters +{ + size_t max_iteration_num{100}; + double traffic_light_signal_timeout{1.0}; + + double backward_path_length; + double forward_path_length; + double backward_length_buffer_for_end_of_pull_over; + double backward_length_buffer_for_end_of_pull_out; + + // common parameters + double min_acc; + double max_acc; + double max_vel; + + double minimum_pull_over_length; + double minimum_pull_out_length; + double drivable_area_resolution; + + double refine_goal_search_radius_range; + + double turn_signal_intersection_search_distance; + double turn_signal_intersection_angle_threshold_deg; + double turn_signal_search_time; + double turn_signal_minimum_search_distance; + double turn_signal_shift_length_threshold; + double turn_signal_remaining_shift_length_threshold; + bool turn_signal_on_swerving; + + bool enable_akima_spline_first; + bool enable_cog_on_centerline; + double input_path_interval; + double output_path_interval; + + double ego_nearest_dist_threshold; + double ego_nearest_yaw_threshold; + + // vehicle info + autoware::vehicle_info_utils::VehicleInfo vehicle_info; + double wheel_base; + double front_overhang; + double rear_overhang; + double vehicle_width; + double vehicle_length; + double wheel_tread; + double left_over_hang; + double right_over_hang; + double base_link2front; + double base_link2rear; +}; + +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp similarity index 94% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp index 7e335b09e4fd3..65ead4019eb1e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include -#include +#include +#include +#include #include -#include #include #include @@ -40,14 +40,14 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using autoware::route_handler::RouteHandler; using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; -using route_handler::RouteHandler; using tier4_planning_msgs::msg::PathWithLaneId; const std::map g_signal_map = { @@ -237,7 +237,7 @@ class TurnSignalDecider inline bool straddleRoadBound( const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, - const vehicle_info_util::VehicleInfo & vehicle_info) const + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) const { using boost::geometry::intersects; using tier4_autoware_utils::pose2transform; @@ -296,6 +296,6 @@ class TurnSignalDecider mutable double intersection_distance_ = std::numeric_limits::max(); mutable Pose intersection_pose_point_ = Pose(); }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp similarity index 89% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp index 7e4cf94c890d8..981e5b495bdd3 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" -#include +#include #include @@ -33,7 +33,7 @@ namespace drivable_area_expansion /// @param[inout] planner_data planning data (params, dynamic objects, vehicle info, ...) void expand_drivable_area( PathWithLaneId & path, - const std::shared_ptr planner_data); + const std::shared_ptr planner_data); /// @brief try to reuse the previous path poses and their previously calculated curvatures /// @details poses are reused if they do not deviate too much from the current path @@ -134,5 +134,5 @@ std::vector calculate_smoothed_curvatures( } // namespace drivable_area_expansion // clang-format off -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT // clang-format on diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp similarity index 80% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index 980da88207f6d..423148c713d10 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -46,4 +46,4 @@ MultiPolygon2d create_object_footprints( const autoware_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp new file mode 100644 index 0000000000000..6216f9216b33b --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp @@ -0,0 +1,44 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ + +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" + +#include + +#include +#include + +namespace drivable_area_expansion +{ +/// @brief Extract uncrossable segments from the lanelet map that are in range of ego +/// @param[in] lanelet_map lanelet map +/// @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 has_types(const lanelet::ConstLineString3d & ls, const std::vector & types); +} // namespace drivable_area_expansion + +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp new file mode 100644 index 0000000000000..35da7e4e24382 --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -0,0 +1,129 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ + +#include +#include + +#include +#include + +namespace drivable_area_expansion +{ + +struct DrivableAreaExpansionParameters +{ + static constexpr auto DRIVABLE_AREA_RIGHT_BOUND_OFFSET_PARAM = "drivable_area_right_bound_offset"; + 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_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 = + "dynamic_expansion.dynamic_objects.extra_footprint_offset.rear"; + static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_LEFT = + "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 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.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 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_ARC_LENGTH_RANGE_PARAM = + "dynamic_expansion.smoothing.arc_length_range"; + static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; + + // 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; + double avoid_linestring_dist{}; + 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 arc_length_range{}; + double max_reuse_deviation{}; + bool avoid_dynamic_objects{}; + bool print_runtime{}; + std::vector avoid_linestring_types{}; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; + + DrivableAreaExpansionParameters() = default; + explicit DrivableAreaExpansionParameters(rclcpp::Node & node) { init(node); } + + void init(rclcpp::Node & node) + { + drivable_area_right_bound_offset = + node.declare_parameter(DRIVABLE_AREA_RIGHT_BOUND_OFFSET_PARAM); + drivable_area_left_bound_offset = + node.declare_parameter(DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM); + drivable_area_types_to_skip = + 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); + arc_length_range = node.declare_parameter(SMOOTHING_ARC_LENGTH_RANGE_PARAM); + max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); + 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 = + node.declare_parameter(DYN_OBJECTS_EXTRA_OFFSET_REAR); + dynamic_objects_extra_left_offset = + node.declare_parameter(DYN_OBJECTS_EXTRA_OFFSET_LEFT); + dynamic_objects_extra_right_offset = + node.declare_parameter(DYN_OBJECTS_EXTRA_OFFSET_RIGHT); + avoid_linestring_types = + 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); + print_runtime = node.declare_parameter(PRINT_RUNTIME_PARAM); + + vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); + } +}; + +} // namespace drivable_area_expansion +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp similarity index 94% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index 2aeac123623ce..c272c4975d802 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -180,4 +180,6 @@ inline LineString2d sub_linestring( } } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +// clang-format off +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ // NOLINT +// clang-format on diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp similarity index 88% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index e47adbd9f6680..6dbe566fb5b6b 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT -#include +#include #include #include #include #include -namespace behavior_path_planner::utils +namespace autoware::behavior_path_planner::utils { using drivable_area_expansion::DrivableAreaExpansionParameters; @@ -100,6 +100,8 @@ std::vector combineDrivableLanes( const std::vector & original_drivable_lanes_vec, const std::vector & new_drivable_lanes_vec); -} // namespace behavior_path_planner::utils +} // namespace autoware::behavior_path_planner::utils -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ +// clang-format off +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT +// clang-format on diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp new file mode 100644 index 0000000000000..5a26cf516c5aa --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ + +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" + +#include +#include +#include +#include + +#include + +#include +#include + +namespace drivable_area_expansion +{ +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::PathPoint; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; + +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 SegmentRtree = boost::geometry::index::rtree>; + +struct PointDistance +{ + Point2d point{}; + double distance{}; +}; +struct Projection +{ + Point2d projected_point{}; + double distance{}; + double arc_length{}; +}; +enum Side { LEFT, RIGHT }; +struct Expansion +{ + // mappings from bound index + std::vector left_distances; + std::vector right_distances; + // mappings from path index + std::vector left_bound_indexes; + std::vector left_projections; + std::vector right_bound_indexes; + std::vector right_projections; + std::vector min_lane_widths; +}; +} // namespace drivable_area_expansion +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp similarity index 89% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 9d18caaa7bd28..91ede1013198d 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ #include @@ -23,7 +23,7 @@ #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { int discretizeAngle(const double theta, const int theta_size); @@ -143,6 +143,6 @@ class OccupancyGridBasedCollisionDetector PlannerWaypoints waypoints_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp new file mode 100644 index 0000000000000..c2b306a4e91db --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -0,0 +1,45 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ + +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include + +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +using autoware_perception_msgs::msg::PredictedObjects; +/* + * 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 autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp similarity index 85% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index a70862756f8e9..739e61481ef2e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/parameters.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/parameters.hpp" -#include +#include #include #include @@ -31,7 +31,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -75,7 +75,8 @@ class GeometricParallelParking bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, - const std::shared_ptr lane_departure_checker); + const std::shared_ptr + autoware_lane_departure_checker); void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } void setPlannerData(const std::shared_ptr & planner_data) { @@ -119,7 +120,8 @@ class GeometricParallelParking const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, const double lane_departure_margin, const double arc_path_interval, - const std::shared_ptr lane_departure_checker); + const std::shared_ptr + autoware_lane_departure_checker); 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); @@ -146,6 +148,8 @@ class GeometricParallelParking Pose start_pose_{}; Pose arc_end_pose_{}; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ +// clang-format off +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT +// clang-format on diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp new file mode 100644 index 0000000000000..781270666f85d --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/utils.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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ + +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include +#include + +namespace autoware::behavior_path_planner::utils::parking_departure +{ + +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; + +std::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 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, + std::optional & stop_pose, const double maximum_deceleration, + const double maximum_jerk); + +/** + * @brief calculate end arc length to generate reference path considering the goal position + * @return a pair of s_end and terminal_is_goal + */ +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose); + +} // namespace autoware::behavior_path_planner::utils::parking_departure + +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp similarity index 94% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index ec5fdb42ead03..4cd9361e10576 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_path_planner::utils::path_safety_checker::filter +namespace autoware::behavior_path_planner::utils::path_safety_checker::filter { using autoware_perception_msgs::msg::PredictedObject; @@ -42,9 +42,9 @@ bool position_filter( const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance); -} // namespace behavior_path_planner::utils::path_safety_checker::filter +} // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter -namespace behavior_path_planner::utils::path_safety_checker +namespace autoware::behavior_path_planner::utils::path_safety_checker { using autoware_perception_msgs::msg::PredictedObject; @@ -318,6 +318,6 @@ void filterObjects(std::vector & objects, Func filter) [[maybe_unused]] std::vector removed_objects{}; filterObjects(objects, removed_objects, filter); } -} // namespace behavior_path_planner::utils::path_safety_checker +} // namespace autoware::behavior_path_planner::utils::path_safety_checker -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp similarity index 95% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 45e3c9e5e9908..e0e01a3a9fbd6 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_path_planner::utils::path_safety_checker +namespace autoware::behavior_path_planner::utils::path_safety_checker { using autoware_perception_msgs::msg::PredictedObject; @@ -236,8 +236,8 @@ using CollisionCheckDebugPair = std::pair; -} // namespace behavior_path_planner::utils::path_safety_checker +} // namespace autoware::behavior_path_planner::utils::path_safety_checker // clang-format off -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT // clang-format on diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp similarity index 87% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index af596ddc293cd..19eebff1b37ed 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -28,19 +28,19 @@ #include -namespace behavior_path_planner::utils::path_safety_checker +namespace autoware::behavior_path_planner::utils::path_safety_checker { +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; -using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; bool isTargetObjectOncoming( const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose, @@ -48,13 +48,13 @@ bool isTargetObjectOncoming( bool isTargetObjectFront( const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::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); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); Polygon2d createExtendedPolygon( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug); Polygon2d createExtendedPolygon( @@ -155,6 +155,6 @@ bool checkSafetyWithIntegralPredictedPolygon( CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); void updateCollisionCheckDebugMap( CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); -} // namespace behavior_path_planner::utils::path_safety_checker +} // namespace autoware::behavior_path_planner::utils::path_safety_checker -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp similarity index 95% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 6651647f8d262..33265d8cef832 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ #include #include @@ -27,7 +27,7 @@ #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -246,6 +246,6 @@ class PathShifter } }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp new file mode 100644 index 0000000000000..c1983f92f54dd --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp @@ -0,0 +1,116 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ + +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner::utils +{ +using autoware_planning_msgs::msg::Path; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathWithLaneId; + +std::vector calcPathArcLengthArray( + const PathWithLaneId & path, const size_t start = 0, + const size_t end = std::numeric_limits::max(), const double offset = 0.0); + +/** + * @brief resample path by spline with constant interval distance + * @param [in] path original path to be resampled + * @param [in] interval constant interval distance + * @param [in] keep_input_points original points are kept in the resampled points + * @param [in] target_section target section defined by arclength if you want to resample a part of + * the path + * @return resampled path + */ +PathWithLaneId resamplePathWithSpline( + const PathWithLaneId & path, const double interval, const bool keep_input_points = false, + const std::pair target_section = {0.0, std::numeric_limits::max()}); + +size_t getIdxByArclength( + const PathWithLaneId & path, const size_t target_idx, const double signed_arc); + +void clipPathLength( + PathWithLaneId & path, const size_t target_idx, const double forward, const double backward); + +void clipPathLength( + PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params); + +std::pair getPathTurnSignal( + const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path, + const ShiftLine & shift_line, const Pose & pose, const double & velocity, + const BehaviorPathPlannerParameters & common_parameter); + +PathWithLaneId convertWayPointsToPathWithLaneId( + const autoware::freespace_planning_algorithms::PlannerWaypoints & waypoints, + const double velocity, const lanelet::ConstLanelets & lanelets); + +std::vector getReversingIndices(const PathWithLaneId & path); + +std::vector dividePath( + const PathWithLaneId & path, const std::vector indices); + +void correctDividedPathVelocity(std::vector & divided_paths); + +bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double distance_threshold); + +// only two points is supported +std::vector splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s); + +std::vector interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval); + +geometry_msgs::msg::Pose getUnshiftedEgoPose( + const geometry_msgs::msg::Pose & ego_pose, const ShiftedPath & prev_path); + +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); + +std::optional getFirstStopPoseFromPath(const PathWithLaneId & path); + +BehaviorModuleOutput getReferencePath( + const lanelet::ConstLanelet & current_lane, + const std::shared_ptr & planner_data); + +BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr & planner_data); + +} // namespace autoware::behavior_path_planner::utils + +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp similarity index 91% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp index 84896cc72daa3..133899d6a478c 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp @@ -11,10 +11,10 @@ // 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_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ -#include +#include #include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_path_planner::utils::traffic_light +namespace autoware::behavior_path_planner::utils::traffic_light { using autoware_perception_msgs::msg::TrafficLightElement; @@ -107,6 +107,6 @@ bool isStoppedAtRedTrafficLightWithinDistance( */ bool isTrafficSignalStop( const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data); -} // namespace behavior_path_planner::utils::traffic_light +} // namespace autoware::behavior_path_planner::utils::traffic_light -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp new file mode 100644 index 0000000000000..e4b8020404b50 --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp @@ -0,0 +1,353 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ + +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "motion_utils/trajectory/trajectory.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner::utils +{ +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; + +using autoware::route_handler::RouteHandler; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Vector3; +using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; + +struct PolygonPoint +{ + geometry_msgs::msg::Point point; + size_t bound_seg_idx{0}; + double lon_dist_to_segment{0.0}; + double lat_dist_to_bound{0.0}; + + bool is_after(const PolygonPoint & other_point) const + { + if (bound_seg_idx == other_point.bound_seg_idx) { + return other_point.lon_dist_to_segment < lon_dist_to_segment; + } + return other_point.bound_seg_idx < bound_seg_idx; + } + + bool is_outside_bounds(const bool is_on_right) const + { + if (is_on_right) { + return lat_dist_to_bound < 0.0; + } + return 0.0 < lat_dist_to_bound; + }; +}; + +struct FrenetPoint +{ + double length{0.0}; // longitudinal + double distance{0.0}; // lateral +}; + +// data conversions +template +FrenetPoint convertToFrenetPoint( + const T & points, const Point & search_point_geom, const size_t seg_idx) +{ + FrenetPoint frenet_point; + + const double longitudinal_length = + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, search_point_geom); + frenet_point.length = motion_utils::calcSignedArcLength(points, 0, seg_idx) + longitudinal_length; + frenet_point.distance = motion_utils::calcLateralOffset(points, search_point_geom, seg_idx); + + return frenet_point; +} + +std::vector getIds(const lanelet::ConstLanelets & lanelets); + +// distance (arclength) calculation + +double l2Norm(const Vector3 vector); + +double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets); + +double getDistanceToNextIntersection( + const Pose & current_pose, const lanelet::ConstLanelets & lanelets); + +double getDistanceToCrosswalk( + const Pose & current_pose, const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphContainer & overall_graphs); + +double getSignedDistance( + const Pose & current_pose, const Pose & goal_pose, const lanelet::ConstLanelets & lanelets); + +double getArcLengthToTargetLanelet( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelet & target_lane, + const Pose & pose); + +double getDistanceBetweenPredictedPaths( + const PredictedPath & path1, const PredictedPath & path2, const double start_time, + const double end_time, const double resolution); + +double getDistanceBetweenPredictedPathAndObject( + const PredictedObject & object, const PredictedPath & path, const double start_time, + const double end_time, const double resolution); + +/** + * @brief Check collision between ego path footprints with extra longitudinal stopping margin and + * objects. + * @return Has collision or not + */ +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); + +/** + * @brief Check collision between ego path footprints and objects. + * @return Has collision or not + */ +bool checkCollisionBetweenPathFootprintsAndObjects( + const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const PathWithLaneId & ego_path, + const PredictedObjects & dynamic_objects, const double margin); + +/** + * @brief Check collision between ego footprints and objects. + * @return Has collision or not + */ +bool checkCollisionBetweenFootprintAndObjects( + const tier4_autoware_utils::LinearRing2d & vehicle_footprint, const Pose & ego_pose, + const PredictedObjects & dynamic_objects, const double margin); + +/** + * @brief calculate lateral distance from ego pose to object + * @return distance from ego pose to object + */ +double calcLateralDistanceFromEgoToObject( + const Pose & ego_pose, const double vehicle_width, const PredictedObject & dynamic_object); + +/** + * @brief calculate longitudinal distance from ego pose to object + * @return distance from ego pose to object + */ +double calcLongitudinalDistanceFromEgoToObject( + const Pose & ego_pose, const double base_link2front, const double base_link2rear, + const PredictedObject & dynamic_object); + +/** + * @brief calculate minimum longitudinal distance from ego pose to objects + * @return minimum distance from ego pose to objects + */ +double calcLongitudinalDistanceFromEgoToObjects( + const Pose & ego_pose, double base_link2front, double base_link2rear, + const PredictedObjects & dynamic_objects); + +// drivable area generation +lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes); +lanelet::ConstLanelets transformToLanelets(const std::vector & drivable_lanes); +std::optional getRightLanelet( + const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes); +std::optional getLeftLanelet( + const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes); +// goal management + +/** + * @brief Modify the path points near the goal to smoothly connect the input path and the goal + * point + * @details Remove the path points that are forward from the goal by the distance of + * search_radius_range. Then insert the goal into the path. The previous goal point generated + * from the goal posture information is also inserted for the smooth connection of the goal pose. + * @param [in] search_radius_range distance on path to be modified for goal insertion + * @param [in] search_rad_range [unused] + * @param [in] input original path + * @param [in] goal original goal pose + * @param [in] goal_lane_id [unused] + * @param [in] output_ptr output path with modified points for the goal + */ +bool setGoal( + const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, + const Pose & goal, const int64_t goal_lane_id, PathWithLaneId * output_ptr); + +/** + * @brief Recreate the goal pose to prevent the goal point being too far from the lanelet, which + * causes the path to twist near the goal. + * @details Return the goal point projected on the straight line of the segment of lanelet + * closest to the original goal. + * @param [in] goal original goal pose + * @param [in] goal_lanelet lanelet containing the goal pose + */ +const Pose refineGoal(const Pose & goal, const lanelet::ConstLanelet & goal_lanelet); + +PathWithLaneId refinePathForGoal( + const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, + const Pose & goal, const int64_t goal_lane_id); + +bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id); + +bool isAllowedGoalModification(const std::shared_ptr & route_handler); +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); + +bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes); + +bool isInLaneletWithYawThreshold( + const Pose & current_pose, const lanelet::ConstLanelet & lanelet, const double yaw_threshold, + const double radius = 0.0); + +bool isEgoOutOfRoute( + const Pose & self_pose, const std::optional & modified_goal, + const std::shared_ptr & route_handler); + +bool isEgoWithinOriginalLane( + const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); + +// path management + +// TODO(Horibe) There is a similar function in route_handler. Check. +std::shared_ptr generateCenterLinePath( + const std::shared_ptr & planner_data); + +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 & 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); + +Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon); + +std::vector getTargetLaneletPolygons( + const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, + const std::string & target_type); + +PathWithLaneId getCenterLinePathFromLanelet( + const lanelet::ConstLanelet & current_route_lanelet, + const std::shared_ptr & planner_data); + +// route handler +PathWithLaneId getCenterLinePath( + const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelet_sequence, + const Pose & pose, const double backward_path_length, const double forward_path_length, + const BehaviorPathPlannerParameters & parameter); + +PathWithLaneId setDecelerationVelocity( + const RouteHandler & route_handler, const PathWithLaneId & input, + const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration, + const double lane_change_buffer); + +// object label +std::uint8_t getHighestProbLabel(const std::vector & classification); + +lanelet::ConstLanelets getCurrentLanes( + const std::shared_ptr & planner_data, const double backward_path_length, + const double forward_path_length); +lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr & planner_data); + +lanelet::ConstLanelets getCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data); + +lanelet::ConstLanelets extendNextLane( + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route = false); + +lanelet::ConstLanelets extendPrevLane( + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route = false); + +lanelet::ConstLanelets extendLanes( + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); + +std::vector getPrecedingLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length); + +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length); + +lanelet::ConstLanelets getExtendedCurrentLanes( + const std::shared_ptr & planner_data); + +lanelet::ConstLanelets getExtendedCurrentLanes( + const std::shared_ptr & planner_data, const double backward_length, + const double forward_length, const bool forward_only_in_route); + +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route); + +lanelet::ConstLanelets calcLaneAroundPose( + const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, + const double forward_length, const double backward_length, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + +bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); + +lanelet::ConstLanelets getLaneletsFromPath( + const PathWithLaneId & path, const std::shared_ptr & route_handler); + +std::string convertToSnakeCase(const std::string & input_str); + +std::optional getPolygonByPoint( + const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, + const std::string & polygon_name); + +template +size_t findNearestSegmentIndex( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, + const double yaw_threshold) +{ + const auto nearest_idx = + motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); + if (nearest_idx) { + return nearest_idx.value(); + } + + return motion_utils::findNearestSegmentIndex(points, pose.position); +} +} // namespace autoware::behavior_path_planner::utils + +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_planner_common/package.xml b/planning/autoware_behavior_path_planner_common/package.xml new file mode 100644 index 0000000000000..6b20027610951 --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/package.xml @@ -0,0 +1,73 @@ + + + + autoware_behavior_path_planner_common + 0.1.0 + The autoware_behavior_path_planner_common package + + + Zulfaqar Azmi + Mamoru Sobue + Satoshi Ota + + + Mamoru Sobue + Daniel Sanchez + Maxime CLEMENT + Takayuki Murooka + Satoshi Ota + + + Tomoya Kimura + Shumpei Wakabayashi + Takayuki Murooka + Kosuke Takeuchi + Fumiya Watanabe + Takamasa Horibe + Zulfaqar Azmi + Go Sakayori + + Apache License 2.0 + + Taiki Tanaka + Takamasa Horibe + Satoshi Ota + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Yutaka Shimizu + Takayuki Murooka + Ryohsuke Mitsudome + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_freespace_planning_algorithms + autoware_lane_departure_checker + autoware_objects_of_interest_marker_interface + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_rtc_interface + autoware_vehicle_info_utils + geometry_msgs + interpolation + lanelet2_extension + magic_enum + motion_utils + object_recognition_utils + rclcpp + tf2 + tier4_autoware_utils + tier4_planning_msgs + traffic_light_utils + visualization_msgs + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp b/planning/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp new file mode 100644 index 0000000000000..f83c73b6a0281 --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp @@ -0,0 +1,25 @@ +// 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 "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" + +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" + +namespace autoware::behavior_path_planner +{ +std::shared_ptr SceneModuleVisitor::getAvoidanceModuleDebugMsg() const +{ + return avoidance_visitor_; +} +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp b/planning/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp similarity index 95% rename from planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp rename to planning/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp index cbced86865a99..a231f00d75fdc 100644 --- a/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp +++ b/planning/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/interface/steering_factor_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp" namespace steering_factor_interface { diff --git a/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp new file mode 100644 index 0000000000000..ce2d412e590b2 --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -0,0 +1,617 @@ +// 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 "autoware_behavior_path_planner_common/marker_utils/utils.hpp" + +#include "autoware_behavior_path_planner_common/marker_utils/colors.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" + +#include +#include +#include + +#include +#include +#include + +#include + +namespace marker_utils +{ +using autoware::behavior_path_planner::utils::calcPathArcLengthArray; +using std_msgs::msg::ColorRGBA; +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; +using visualization_msgs::msg::Marker; + +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + 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(pose, base_to_front, -half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + marker.points.push_back(marker.points.front()); +} + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.2, 0.2, 0.2), + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + + addFootprintMarker(marker, base_link_pose, vehicle_info); + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & point : points) { + marker.points.push_back(point); + } + + MarkerArray msg; + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPoseMarkerArray( + const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, + const float & b) +{ + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::ARROW, + createMarkerScale(0.7, 0.3, 0.3), createMarkerColor(r, g, b, 0.999)); + marker.pose = pose; + msg.markers.push_back(marker); + + return msg; +} + +MarkerArray createPathMarkerArray( + const PathWithLaneId & path, std::string && ns, const int64_t & lane_id, const float & r, + const float & g, const float & b) +{ + const auto arclength = calcPathArcLengthArray(path); + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + const int32_t uid = bitShift(lane_id); + int32_t i{0}; + int32_t idx{0}; + + MarkerArray msg; + const auto reserve_size = path.points.size() + static_cast(path.points.size() / 10); + msg.markers.reserve(reserve_size); + + Marker marker = createDefaultMarker( + "map", current_time, ns, 0L, Marker::ARROW, createMarkerScale(0.2, 0.1, 0.3), + createMarkerColor(r, g, b, 0.999)); + + Marker marker_text = createDefaultMarker( + "map", current_time, ns, 0L, Marker::TEXT_VIEW_FACING, createMarkerScale(0.2, 0.1, 0.3), + createMarkerColor(1, 1, 1, 0.999)); + + for (const auto & p : path.points) { + marker.id = uid + i++; + marker.pose = p.point.pose; + msg.markers.push_back(marker); + if (idx % 10 == 0) { + marker_text.id = uid + i++; + std::stringstream ss; + ss << std::fixed << std::setprecision(1) << "i=" << idx << "\ns=" << arclength.at(idx); + marker_text.text = ss.str(); + msg.markers.push_back(marker_text); + } + ++idx; + } + + return msg; +} +MarkerArray createShiftLineMarkerArray( + const ShiftLineArray & shift_lines, const double & base_shift, std::string && ns, const float & r, + const float & g, const float & b, const float & w) +{ + ShiftLineArray shift_lines_local = shift_lines; + if (shift_lines.empty()) { + shift_lines_local.emplace_back(); + } + + MarkerArray msg; + msg.markers.reserve(shift_lines_local.size() * 3); + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + int id{0}; + + // TODO(Horibe) now assuming the shift point is aligned in longitudinal distance order + double current_shift = base_shift; + for (const auto & sp : shift_lines_local) { + // ROS_ERROR("sp: s = (%f, %f), g = (%f, %f)", sp.start.x, sp.start.y, sp.end.x, sp.end.y); + Marker basic_marker = createDefaultMarker( + "map", current_time, ns, 0L, Marker::CUBE, createMarkerScale(0.5, 0.5, 0.5), + createMarkerColor(r, g, b, 0.5)); + basic_marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + { + // start point + auto marker_s = basic_marker; + marker_s.id = ++id; + marker_s.pose = sp.start; + marker_s.pose = calcOffsetPose(marker_s.pose, 0.0, current_shift, 0.0); + msg.markers.push_back(marker_s); + + // end point + auto marker_e = basic_marker; + marker_e.id = ++id; + marker_e.pose = sp.end; + marker_e.pose = calcOffsetPose(marker_e.pose, 0.0, sp.end_shift_length, 0.0); + msg.markers.push_back(marker_e); + + // start-to-end line + auto marker_l = basic_marker; + marker_l.id = ++id; + marker_l.type = Marker::LINE_STRIP; + marker_l.scale = createMarkerScale(w, 0.0, 0.0); + marker_l.points.reserve(2); + marker_l.points.push_back(marker_s.pose.position); + marker_l.points.push_back(marker_e.pose.position); + msg.markers.push_back(marker_l); + } + current_shift = sp.end_shift_length; + } + + return msg; +} + +MarkerArray createShiftLengthMarkerArray( + const std::vector & shift_distance, const PathWithLaneId & reference, std::string && ns, + const float & r, const float & g, const float & b) +{ + if (shift_distance.size() != reference.points.size()) { + return MarkerArray{}; + } + + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.9)); + marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + marker.points.reserve(shift_distance.size()); + + for (size_t i = 0; i < shift_distance.size(); ++i) { + const auto p = + calcOffsetPose(reference.points.at(i).point.pose, 0.0, shift_distance.at(i), 0.0); + marker.points.push_back(p.position); + } + + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createShiftGradMarkerArray( + const std::vector & grad, const std::vector & shift_distance, + const PathWithLaneId & reference, std::string && ns, const float & r, const float & g, + const float & b) +{ + if (grad.size() != reference.points.size()) { + return MarkerArray{}; + } + + if (shift_distance.size() != reference.points.size()) { + return MarkerArray{}; + } + + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(r, g, b, 0.9)); + + for (size_t i = 0; i < grad.size(); ++i) { + std::ostringstream string_stream; + string_stream << "Grad:" << grad.at(i); + marker.text = string_stream.str(); + marker.pose = calcOffsetPose(reference.points.at(i).point.pose, 0.0, shift_distance.at(i), 0.0); + marker.id = i; + + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createLaneletsAreaMarkerArray( + const std::vector & lanelets, std::string && ns, const float & r, + const float & g, const float & b) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + for (const auto & lanelet : lanelets) { + Marker marker = createDefaultMarker( + + "map", current_time, ns, static_cast(lanelet.id()), Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + + if (!lanelet.polygon3d().empty()) { + marker.points.reserve(lanelet.polygon3d().size() + 1); + } + + for (const auto & p : lanelet.polygon3d()) { + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createPolygonMarkerArray( + const Polygon & polygon, std::string && ns, const int64_t & lane_id, const float & r, + const float & g, const float & b, const float & w) +{ + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, static_cast(lane_id), Marker::LINE_STRIP, + createMarkerScale(w, 0.0, 0.0), createMarkerColor(r, g, b, 0.8)); + + marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + + if (!polygon.points.empty()) { + marker.points.reserve(polygon.points.size() + 1); + } + + for (const auto & p : polygon.points) { + marker.points.push_back(createPoint(p.x, p.y, p.z)); + } + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + + MarkerArray msg; + msg.markers.push_back(marker); + + return msg; +} + +MarkerArray createObjectsMarkerArray( + const PredictedObjects & objects, std::string && ns, const int64_t & lane_id, const float & r, + const float & g, const float & b) +{ + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, + createMarkerScale(3.0, 1.0, 1.0), createMarkerColor(r, g, b, 0.8)); + + int32_t uid = bitShift(lane_id); + int32_t i{0}; + + MarkerArray msg; + msg.markers.reserve(objects.objects.size()); + + for (const auto & object : objects.objects) { + marker.id = uid + i++; + marker.pose = object.kinematics.initial_pose_with_covariance.pose; + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createDrivableLanesMarkerArray( + const std::vector & drivable_lanes, std::string && ns) +{ + MarkerArray msg; + + int32_t i{0}; + + const auto get_lanelet_marker = + [&ns, &i](const auto & lanelet, const auto r, const auto g, const auto b) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, bitShift(lanelet.id()) + i++, + Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + if (lanelet.polygon3d().empty()) { + return marker; + } + + marker.points.reserve(lanelet.polygon3d().size() + 1); + + for (const auto & p : lanelet.polygon3d()) { + marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + } + + marker.points.push_back(marker.points.front()); + + return marker; + }; + + const auto get_drivable_lanes = [&msg, &get_lanelet_marker](const DrivableLanes & drivable_lane) { + { + msg.markers.push_back(get_lanelet_marker(drivable_lane.right_lane, 1.0, 0.0, 0.0)); + } + + { + msg.markers.push_back(get_lanelet_marker(drivable_lane.left_lane, 0.0, 1.0, 0.0)); + } + + std::for_each( + drivable_lane.middle_lanes.begin(), drivable_lane.middle_lanes.end(), + [&](const auto & lane) { msg.markers.push_back(get_lanelet_marker(lane, 0.0, 0.0, 1.0)); }); + }; + + std::for_each(drivable_lanes.begin(), drivable_lanes.end(), get_drivable_lanes); + + return msg; +} +MarkerArray createPredictedPathMarkerArray( + const PredictedPath & predicted_path, + const autoware::vehicle_info_utils::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); + addFootprintMarker(marker, predicted_path_pose, vehicle_info); + + marker_array.markers.push_back(marker); + } + return marker_array; +} + +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 autoware::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_common/src/turn_signal_decider.cpp b/planning/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp similarity index 99% rename from planning/behavior_path_planner_common/src/turn_signal_decider.cpp rename to planning/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index 146fb03cc00f2..f3a6e91818617 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include #include @@ -31,7 +31,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using motion_utils::calcSignedArcLength; @@ -790,4 +790,4 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( return std::make_pair(turn_signal_info, false); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp similarity index 96% rename from planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 42c2a375d77cf..cb938f82f2469 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include #include @@ -359,7 +359,7 @@ void calculate_expansion_distances( void expand_drivable_area( PathWithLaneId & path, - const std::shared_ptr planner_data) + 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; diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp similarity index 93% rename from planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index 8565d737d5762..4159ce5000b66 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" #include #include diff --git a/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp new file mode 100644 index 0000000000000..5f9eb817cd049 --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp @@ -0,0 +1,54 @@ +// 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 "autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" + +#include + +#include +#include + +#include + +namespace drivable_area_expansion +{ +SegmentRtree extract_uncrossable_segments( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params) +{ + 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 (has_types(ls, params.avoid_linestring_types)) { + line.clear(); + 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 uncrossable_segments_in_range; +} + +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); + return (type != no_type && std::find(types.begin(), types.end(), type) != types.end()); +} +} // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp similarity index 99% rename from planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 571a40859202f..b802946d627bd 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -11,9 +11,9 @@ // 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_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -232,7 +232,7 @@ std::vector extractBoundFromPolygon( } } // namespace -namespace behavior_path_planner::utils::drivable_area_processing +namespace autoware::behavior_path_planner::utils::drivable_area_processing { std::optional> intersectBound( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, @@ -636,9 +636,9 @@ std::vector updateBoundary( return center_pos; } -} // namespace behavior_path_planner::utils::drivable_area_processing +} // namespace autoware::behavior_path_planner::utils::drivable_area_processing -namespace behavior_path_planner::utils +namespace autoware::behavior_path_planner::utils { using tier4_autoware_utils::Point2d; @@ -1822,4 +1822,4 @@ lanelet::ConstLanelets combineLanelets( return combined_lanes; } -} // namespace behavior_path_planner::utils +} // namespace autoware::behavior_path_planner::utils diff --git a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp similarity index 96% rename from planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 7dae0fb32c79f..d9cadb1678cb3 100644 --- a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using tier4_autoware_utils::createQuaternionFromYaw; using tier4_autoware_utils::normalizeRadian; @@ -214,4 +214,4 @@ bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( return false; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp similarity index 97% rename from planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 90790f61db1a0..f4c5b1ce36872 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -41,7 +41,7 @@ using tier4_autoware_utils::normalizeRadian; using tier4_autoware_utils::transformPose; using tier4_planning_msgs::msg::PathWithLaneId; -namespace behavior_path_planner +namespace autoware::behavior_path_planner { void GeometricParallelParking::incrementPathIndex() { @@ -223,7 +223,8 @@ 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 bool left_side_start, - const std::shared_ptr lane_departure_checker) + const std::shared_ptr + lane_departure_checker) { constexpr bool is_forward = false; // parking backward means pull_out forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose @@ -364,7 +365,8 @@ std::vector GeometricParallelParking::planOneTrial( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, const double lane_departure_margin, const double arc_path_interval, - const std::shared_ptr lane_departure_checker) + const std::shared_ptr + lane_departure_checker) { clearPaths(); @@ -610,4 +612,4 @@ void GeometricParallelParking::setTurningRadius( common_params.wheel_base + common_params.front_overhang); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp new file mode 100644 index 0000000000000..642ad10b341dd --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -0,0 +1,180 @@ +// 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 "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" + +#include "autoware_behavior_path_planner_common/utils/utils.hpp" + +#include +#include + +namespace autoware::behavior_path_planner::utils::parking_departure +{ + +using motion_utils::calcDecelDistWithJerkAndAccConstraints; + +std::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 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. + const double min_accel_for_ego_predicted_path = ego_predicted_path_params->min_acceleration; + 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, + std::optional & 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 = + autoware::behavior_path_planner::utils::parking_departure::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; +} + +std::pair calcEndArcLength( + const double s_start, const double forward_path_length, const lanelet::ConstLanelets & road_lanes, + const Pose & goal_pose) +{ + const double s_forward_length = s_start + forward_path_length; + // use forward length if the goal pose is not in the lanelets + if (!utils::isInLanelets(goal_pose, road_lanes)) { + return {s_forward_length, false}; + } + + const double s_goal = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + + // If the goal is behind the start or beyond the forward length, use forward length. + if (s_goal < s_start || s_goal >= s_forward_length) { + return {s_forward_length, false}; + } + + // path end is goal + return {s_goal, true}; +} + +} // namespace autoware::behavior_path_planner::utils::parking_departure diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp similarity index 97% rename from planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 3abd9b6eb0c05..6454800dff1f6 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include @@ -24,7 +24,7 @@ #include -namespace behavior_path_planner::utils::path_safety_checker::filter +namespace autoware::behavior_path_planner::utils::path_safety_checker::filter { bool velocity_filter(const PredictedObject & object, double velocity_threshold, double max_velocity) { @@ -53,9 +53,9 @@ bool is_within_circle( std::hypot(reference_point.x - object_pos.x, reference_point.y - object_pos.y); return dist < search_radius; } -} // namespace behavior_path_planner::utils::path_safety_checker::filter +} // namespace autoware::behavior_path_planner::utils::path_safety_checker::filter -namespace behavior_path_planner::utils::path_safety_checker +namespace autoware::behavior_path_planner::utils::path_safety_checker { bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { @@ -410,4 +410,4 @@ bool isTargetObjectType( (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); } -} // namespace behavior_path_planner::utils::path_safety_checker +} // namespace autoware::behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp similarity index 97% rename from planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index e1884e57f7221..8a9415285037d 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" @@ -28,7 +28,7 @@ #include -namespace behavior_path_planner::utils::path_safety_checker +namespace autoware::behavior_path_planner::utils::path_safety_checker { namespace bg = boost::geometry; @@ -51,7 +51,7 @@ bool isTargetObjectOncoming( bool isTargetObjectFront( const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_offset_pose = @@ -71,7 +71,7 @@ bool isTargetObjectFront( bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_point = @@ -90,7 +90,7 @@ bool isTargetObjectFront( } Polygon2d createExtendedPolygon( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { @@ -189,7 +189,7 @@ Polygon2d createExtendedPolygon( Polygon2d createExtendedPolygonAlongPath( const PathWithLaneId & planned_path, const Pose & base_link_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lon_length, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { const double & base_to_front = vehicle_info.max_longitudinal_offset_m; @@ -710,4 +710,4 @@ void updateCollisionCheckDebugMap( debug_map.insert(object_debug); } -} // namespace behavior_path_planner::utils::path_safety_checker +} // namespace autoware::behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp similarity index 98% rename from planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 1ea555f4675ef..0b21d500b164b 100644 --- a/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include #include @@ -31,7 +31,7 @@ std::string toStr(const geometry_msgs::msg::Point & p) { return "(" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + ")"; } -std::string toStr(const behavior_path_planner::ShiftLine & p) +std::string toStr(const autoware::behavior_path_planner::ShiftLine & p) { return "start point = " + toStr(p.start.position) + ", end point = " + toStr(p.end.position) + ", start idx = " + std::to_string(p.start_idx) + @@ -48,7 +48,7 @@ std::string toStr(const std::vector & v) } } // namespace -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using motion_utils::findNearestIndex; @@ -648,4 +648,4 @@ std::optional PathShifter::getLastShiftLine() const return *furthest; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp new file mode 100644 index 0000000000000..e4dd9a401253f --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -0,0 +1,687 @@ +// 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 "autoware_behavior_path_planner_common/utils/path_utils.hpp" + +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_path_planner::utils +{ +/** + * @brief calc path arclength on each points from start point to end point. + */ +std::vector calcPathArcLengthArray( + const PathWithLaneId & path, const size_t start, const size_t end, const double offset) +{ + const auto bounded_start = std::max(start, size_t{0}); + const auto bounded_end = std::min(end, path.points.size()); + std::vector out; + out.reserve(bounded_end - bounded_start); + + double sum = offset; + out.push_back(sum); + + for (size_t i = bounded_start + 1; i < bounded_end; ++i) { + sum += + tier4_autoware_utils::calcDistance2d(path.points.at(i).point, path.points.at(i - 1).point); + out.push_back(sum); + } + return out; +} + +/** + * @brief resamplePathWithSpline + */ +PathWithLaneId resamplePathWithSpline( + const PathWithLaneId & path, const double interval, const bool keep_input_points, + const std::pair target_section) +{ + if (path.points.size() < 2) { + return path; + } + + std::vector transformed_path(path.points.size()); + for (size_t i = 0; i < path.points.size(); ++i) { + transformed_path.at(i) = path.points.at(i).point; + } + + const auto find_almost_same_values = + [&](const std::vector & vec, double x) -> std::optional> { + constexpr double epsilon = 0.2; + const auto is_close = [&](double v, double x) { return std::abs(v - x) < epsilon; }; + + std::vector indices; + if (vec.empty()) { + return std::nullopt; + } + + for (size_t i = 0; i < vec.size(); ++i) { + if (is_close(vec[i], x)) { + indices.push_back(i); + } + } + + if (indices.empty()) { + return std::nullopt; + } + + return indices; + }; + + // Get lane ids that are not duplicated + std::vector s_in; + std::unordered_set unique_lane_ids; + const auto s_vec = + motion_utils::calcSignedArcLengthPartialSum(transformed_path, 0, transformed_path.size()); + for (size_t i = 0; i < path.points.size(); ++i) { + const double s = s_vec.at(i); + for (const auto & lane_id : path.points.at(i).lane_ids) { + if (!keep_input_points && (unique_lane_ids.find(lane_id) != unique_lane_ids.end())) { + continue; + } + unique_lane_ids.insert(lane_id); + + if (!find_almost_same_values(s_in, s)) { + s_in.push_back(s); + } + } + } + + std::vector s_out = s_in; + + // sampling from interval distance + const auto start_s = std::max(target_section.first, 0.0); + const auto end_s = std::min(target_section.second, s_vec.back()); + for (double s = start_s; s < end_s; s += interval) { + if (!find_almost_same_values(s_out, s)) { + s_out.push_back(s); + } + } + if (!find_almost_same_values(s_out, end_s)) { + s_out.push_back(end_s); + } + + // Insert Stop Point + const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint(transformed_path); + if (closest_stop_dist) { + const auto close_indices = find_almost_same_values(s_out, *closest_stop_dist); + if (close_indices) { + // Update the smallest index + s_out.at(close_indices->at(0)) = *closest_stop_dist; + + // Remove the rest of the indices in descending order + for (size_t i = close_indices->size() - 1; i > 0; --i) { + s_out.erase(s_out.begin() + close_indices->at(i)); + } + } else { + s_out.push_back(*closest_stop_dist); + } + } + + // spline resample required more than 2 points for yaw angle calculation + if (s_out.size() < 2) { + return path; + } + + std::sort(s_out.begin(), s_out.end()); + + return motion_utils::resamplePath(path, s_out); +} + +size_t getIdxByArclength( + const PathWithLaneId & path, const size_t target_idx, const double signed_arc) +{ + if (path.points.empty()) { + throw std::runtime_error("[getIdxByArclength] path points must be > 0"); + } + + using tier4_autoware_utils::calcDistance2d; + double sum_length = 0.0; + if (signed_arc >= 0.0) { + for (size_t i = target_idx; i < path.points.size() - 1; ++i) { + const auto next_i = i + 1; + sum_length += calcDistance2d(path.points.at(i), path.points.at(next_i)); + if (sum_length > signed_arc) { + return next_i; + } + } + return path.points.size() - 1; + } + for (size_t i = target_idx; i > 0; --i) { + const auto next_i = i - 1; + sum_length -= calcDistance2d(path.points.at(i), path.points.at(next_i)); + if (sum_length < signed_arc) { + return next_i; + } + } + return 0; +} + +// TODO(murooka) This function should be replaced with motion_utils::cropPoints +void clipPathLength( + PathWithLaneId & path, const size_t target_idx, const double forward, const double backward) +{ + if (path.points.size() < 3) { + return; + } + + const auto start_idx = utils::getIdxByArclength(path, target_idx, -backward); + const auto end_idx = utils::getIdxByArclength(path, target_idx, forward); + + const std::vector clipped_points{ + path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; + + path.points = clipped_points; +} + +// TODO(murooka) This function should be replaced with motion_utils::cropPoints +void clipPathLength( + PathWithLaneId & path, const size_t target_idx, const BehaviorPathPlannerParameters & params) +{ + clipPathLength(path, target_idx, params.forward_path_length, params.backward_path_length); +} + +std::pair getPathTurnSignal( + const lanelet::ConstLanelets & current_lanes, const ShiftedPath & path, + const ShiftLine & shift_line, const Pose & pose, const double & velocity, + const BehaviorPathPlannerParameters & common_parameter) +{ + TurnIndicatorsCommand turn_signal; + 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.size() < shift_line.end_idx + 1) { + return std::make_pair(turn_signal, max_distance); + } + const auto base_link2front = common_parameter.base_link2front; + const auto vehicle_width = common_parameter.vehicle_width; + const auto shift_to_outside = vehicle_width / 2; + const auto turn_signal_shift_length_threshold = + common_parameter.turn_signal_shift_length_threshold; + const auto turn_signal_minimum_search_distance = + common_parameter.turn_signal_minimum_search_distance; + const auto turn_signal_search_time = common_parameter.turn_signal_search_time; + constexpr double epsilon = 1e-6; + const auto arc_position_current_pose = lanelet::utils::getArcCoordinates(current_lanes, pose); + + // Start turn signal when 1 or 2 is satisfied + // 1. time to shift start point is less than prev_sec + // 2. distance to shift point is shorter than tl_on_threshold_long + + // Turn signal on when conditions below are satisfied + // 1. lateral offset is larger than tl_on_threshold_lat for left signal + // smaller than tl_on_threshold_lat for right signal + // 2. side point at shift start/end point cross the line + const double distance_to_shift_start = + std::invoke([¤t_lanes, &shift_line, &arc_position_current_pose]() { + const auto arc_position_shift_start = + lanelet::utils::getArcCoordinates(current_lanes, shift_line.start); + return arc_position_shift_start.length - arc_position_current_pose.length; + }); + + const auto time_to_shift_start = + (std::abs(velocity) < epsilon) ? max_time : distance_to_shift_start / velocity; + + const double diff = + path.shift_length.at(shift_line.end_idx) - path.shift_length.at(shift_line.start_idx); + + Pose shift_start_point = path.path.points.at(shift_line.start_idx).point.pose; + Pose shift_end_point = path.path.points.at(shift_line.end_idx).point.pose; + Pose left_start_point = shift_start_point; + Pose right_start_point = shift_start_point; + Pose left_end_point = shift_end_point; + Pose right_end_point = shift_end_point; + { + const double start_yaw = tf2::getYaw(shift_line.start.orientation); + const double end_yaw = tf2::getYaw(shift_line.end.orientation); + left_start_point.position.x -= std::sin(start_yaw) * (shift_to_outside); + left_start_point.position.y += std::cos(start_yaw) * (shift_to_outside); + right_start_point.position.x -= std::sin(start_yaw) * (-shift_to_outside); + right_start_point.position.y += std::cos(start_yaw) * (-shift_to_outside); + left_end_point.position.x -= std::sin(end_yaw) * (shift_to_outside); + left_end_point.position.y += std::cos(end_yaw) * (shift_to_outside); + right_end_point.position.x -= std::sin(end_yaw) * (-shift_to_outside); + right_end_point.position.y += std::cos(end_yaw) * (-shift_to_outside); + } + + bool left_start_point_is_in_lane = false; + bool right_start_point_is_in_lane = false; + bool left_end_point_is_in_lane = false; + bool right_end_point_is_in_lane = false; + { + for (const auto & llt : current_lanes) { + if (lanelet::utils::isInLanelet(left_start_point, llt, 0.1)) { + left_start_point_is_in_lane = true; + } + if (lanelet::utils::isInLanelet(right_start_point, llt, 0.1)) { + right_start_point_is_in_lane = true; + } + if (lanelet::utils::isInLanelet(left_end_point, llt, 0.1)) { + left_end_point_is_in_lane = true; + } + if (lanelet::utils::isInLanelet(right_end_point, llt, 0.1)) { + right_end_point_is_in_lane = true; + } + } + } + + const bool cross_line = std::invoke([&]() { + constexpr bool temporary_set_cross_line_true = + true; // due to a bug. See link: + // https://github.com/autowarefoundation/autoware.universe/pull/748 + if (temporary_set_cross_line_true) { + return true; + } + return ( + left_start_point_is_in_lane != left_end_point_is_in_lane || + right_start_point_is_in_lane != right_end_point_is_in_lane); + }); + + if ( + time_to_shift_start < turn_signal_search_time || + distance_to_shift_start < turn_signal_minimum_search_distance) { + if (diff > turn_signal_shift_length_threshold && cross_line) { + turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else if (diff < -turn_signal_shift_length_threshold && cross_line) { + turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + } + + // calc distance from ego vehicle front to shift end point. + const double distance_from_vehicle_front = + std::invoke([¤t_lanes, &shift_line, &arc_position_current_pose, &base_link2front]() { + const auto arc_position_shift_end = + lanelet::utils::getArcCoordinates(current_lanes, shift_line.end); + return arc_position_shift_end.length - arc_position_current_pose.length - base_link2front; + }); + + if (distance_from_vehicle_front >= 0.0) { + return std::make_pair(turn_signal, distance_from_vehicle_front); + } + + return std::make_pair(turn_signal, max_distance); +} + +PathWithLaneId convertWayPointsToPathWithLaneId( + const autoware::freespace_planning_algorithms::PlannerWaypoints & waypoints, + const double velocity, const lanelet::ConstLanelets & lanelets) +{ + PathWithLaneId path; + path.header = waypoints.header; + for (size_t i = 0; i < waypoints.waypoints.size(); ++i) { + const auto & waypoint = waypoints.waypoints.at(i); + PathPointWithLaneId point{}; + point.point.pose = waypoint.pose.pose; + // put the lane that contain waypoints in lane_ids. + bool is_in_lanes = false; + for (const auto & lane : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lane)) { + point.lane_ids.push_back(lane.id()); + is_in_lanes = true; + } + } + // If none of them corresponds, assign the previous lane_ids. + if (!is_in_lanes && i > 0) { + point.lane_ids = path.points.at(i - 1).lane_ids; + } + + point.point.longitudinal_velocity_mps = (waypoint.is_back ? -1 : 1) * velocity; + path.points.push_back(point); + } + return path; +} + +std::vector getReversingIndices(const PathWithLaneId & path) +{ + std::vector indices; + + for (size_t i = 0; i < path.points.size() - 1; ++i) { + if ( + path.points.at(i).point.longitudinal_velocity_mps * + path.points.at(i + 1).point.longitudinal_velocity_mps < + 0) { + indices.push_back(i); + } + } + + return indices; +} + +std::vector dividePath( + const PathWithLaneId & path, const std::vector indices) +{ + std::vector divided_paths; + + if (indices.empty()) { + divided_paths.push_back(path); + return divided_paths; + } + + for (size_t i = 0; i < indices.size(); ++i) { + PathWithLaneId divided_path; + divided_path.header = path.header; + if (i == 0) { + divided_path.points.insert( + divided_path.points.end(), path.points.begin(), path.points.begin() + indices.at(i) + 1); + } else { + // include the point at indices.at(i - 1) and indices.at(i) + divided_path.points.insert( + divided_path.points.end(), path.points.begin() + indices.at(i - 1), + path.points.begin() + indices.at(i) + 1); + } + divided_paths.push_back(divided_path); + } + + PathWithLaneId divided_path; + divided_path.header = path.header; + divided_path.points.insert( + divided_path.points.end(), path.points.begin() + indices.back(), path.points.end()); + divided_paths.push_back(divided_path); + + return divided_paths; +} + +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; + } + + if (*is_driving_forward) { + for (auto & point : path.points) { + point.point.longitudinal_velocity_mps = std::abs(point.point.longitudinal_velocity_mps); + } + } else { + for (auto & point : path.points) { + point.point.longitudinal_velocity_mps = -std::abs(point.point.longitudinal_velocity_mps); + } + } + path.points.back().point.longitudinal_velocity_mps = 0.0; + } +} + +bool isCloseToPath(const PathWithLaneId & path, const Pose & pose, const double distance_threshold) +{ + for (const auto & point : path.points) { + const auto & p = point.point.pose.position; + const double dist = std::hypot(pose.position.x - p.x, pose.position.y - p.y); + if (dist < distance_threshold) { + return true; + } + } + return false; +} + +// only two points is supported +std::vector splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s) +{ + assert(base_s.size() == 2 && base_x.size() == 2); + + const double h = base_s.at(1) - base_s.at(0); + + const double c = begin_diff; + const double d = base_x.at(0); + const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); + const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); + + std::vector res; + for (const auto & s : new_s) { + const double ds = s - base_s.at(0); + res.push_back(d + (c + (b + a * ds) * ds) * ds); + } + + return res; +} + +std::vector interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval) +{ + std::vector interpolated_poses{}; // output + + const std::vector base_s{ + 0, tier4_autoware_utils::calcDistance2d(start_pose.position, end_pose.position)}; + const std::vector base_x{start_pose.position.x, end_pose.position.x}; + const std::vector base_y{start_pose.position.y, end_pose.position.y}; + std::vector new_s; + + constexpr double eps = 0.3; // prevent overlapping + for (double s = eps; s < base_s.back() - eps; s += resample_interval) { + new_s.push_back(s); + } + + const std::vector interpolated_x = splineTwoPoints( + base_s, base_x, std::cos(tf2::getYaw(start_pose.orientation)), + std::cos(tf2::getYaw(end_pose.orientation)), new_s); + const std::vector interpolated_y = splineTwoPoints( + base_s, base_y, std::sin(tf2::getYaw(start_pose.orientation)), + std::sin(tf2::getYaw(end_pose.orientation)), new_s); + for (size_t i = 0; i < interpolated_x.size(); ++i) { + Pose pose{}; + pose = tier4_autoware_utils::calcInterpolatedPose( + end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); + pose.position.x = interpolated_x.at(i); + pose.position.y = interpolated_y.at(i); + pose.position.z = end_pose.position.z; + interpolated_poses.push_back(pose); + } + + return interpolated_poses; +} + +Pose getUnshiftedEgoPose(const Pose & ego_pose, const ShiftedPath & prev_path) +{ + if (prev_path.path.points.empty()) { + return ego_pose; + } + + // un-shifted for current ideal pose + const auto closest_idx = motion_utils::findNearestIndex(prev_path.path.points, ego_pose.position); + + // NOTE: Considering avoidance by motion, we set unshifted_pose as previous path instead of + // ego_pose. + auto unshifted_pose = motion_utils::calcInterpolatedPoint(prev_path.path, ego_pose).point.pose; + + unshifted_pose = tier4_autoware_utils::calcOffsetPose( + unshifted_pose, 0.0, -prev_path.shift_length.at(closest_idx), 0.0); + unshifted_pose.orientation = ego_pose.orientation; + + return unshifted_pose; +} + +// TODO(Horibe) clean up functions: there is a similar code in util as well. +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) +{ + const auto & p = planner_data->parameters; + const auto & route_handler = planner_data->route_handler; + + PathWithLaneId centerline_path; + + const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. + const auto backward_length = + std::max(p.backward_path_length, longest_dist_to_shift_line + extra_margin); + + RCLCPP_DEBUG( + rclcpp::get_logger("path_utils"), + "p.backward_path_length = %f, longest_dist_to_shift_line = %f, backward_length = %f", + p.backward_path_length, longest_dist_to_shift_line, backward_length); + + const lanelet::ConstLanelets current_lanes = [&]() { + if (!prev_module_path) { + return utils::calcLaneAroundPose( + route_handler, ref_pose, p.forward_path_length, backward_length); + } + return utils::getCurrentLanesFromPath(*prev_module_path, planner_data); + }(); + + centerline_path = utils::getCenterLinePath( + *route_handler, current_lanes, ref_pose, backward_length, p.forward_path_length, p); + + centerline_path.header = route_handler->getRouteHeader(); + + 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; +} + +std::optional getFirstStopPoseFromPath(const PathWithLaneId & path) +{ + for (const auto & p : path.points) { + if (std::abs(p.point.longitudinal_velocity_mps) < 0.01) { + return p.point.pose; + } + } + return std::nullopt; +} + +BehaviorModuleOutput getReferencePath( + const lanelet::ConstLanelet & current_lane, + const std::shared_ptr & planner_data) +{ + PathWithLaneId reference_path{}; + + const auto & route_handler = planner_data->route_handler; + const auto current_pose = planner_data->self_odometry->pose.pose; + const auto p = planner_data->parameters; + + // Set header + reference_path.header = route_handler->getRouteHeader(); + + // calculate path with backward margin to avoid end points' instability by spline interpolation + constexpr double extra_margin = 10.0; + const double backward_length = p.backward_path_length + extra_margin; + const auto current_lanes_with_backward_margin = + route_handler->getLaneletSequence(current_lane, backward_length, p.forward_path_length); + const auto no_shift_pose = + lanelet::utils::getClosestCenterPose(current_lane, current_pose.position); + reference_path = getCenterLinePath( + *route_handler, current_lanes_with_backward_margin, no_shift_pose, backward_length, + p.forward_path_length, p); + + // 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::findFirstNearestSegmentIndexWithSoftConstraints( + reference_path.points, no_shift_pose, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold); + reference_path.points = motion_utils::cropPoints( + reference_path.points, no_shift_pose.position, current_seg_idx, p.forward_path_length, + p.backward_path_length + p.input_path_interval); + + const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler); + const auto drivable_lanes = generateDrivableLanes(drivable_lanelets); + + const auto & dp = planner_data->drivable_area_expansion_parameters; + + const auto shorten_lanes = cutOverlappedLanes(reference_path, drivable_lanes); + const auto expanded_lanes = expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + + BehaviorModuleOutput output; + output.path = reference_path; + output.reference_path = reference_path; + output.drivable_area_info.drivable_lanes = drivable_lanes; + + return output; +} + +BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr & planner_data) +{ + BehaviorModuleOutput output; + + const auto & route_handler = planner_data->route_handler; + const auto & modified_goal = planner_data->prev_modified_goal; + + const Pose goal_pose = modified_goal ? modified_goal->pose : route_handler->getGoalPose(); + + lanelet::ConstLanelet goal_lane; + const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose); + if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front(); + const auto is_failed_getting_lanelet = + shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane); + if (is_failed_getting_lanelet) { + return output; + } + + constexpr double backward_length = 1.0; + const auto arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, goal_pose); + const double s_start = std::max(arc_coord.length - backward_length, 0.0); + const double s_end = arc_coord.length; + + auto reference_path = route_handler->getCenterLinePath({goal_lane}, s_start, s_end); + + const auto drivable_lanelets = getLaneletsFromPath(reference_path, route_handler); + const auto drivable_lanes = generateDrivableLanes(drivable_lanelets); + + const auto & dp = planner_data->drivable_area_expansion_parameters; + + const auto shorten_lanes = cutOverlappedLanes(reference_path, drivable_lanes); + const auto expanded_lanes = expandLanelets( + shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, + dp.drivable_area_types_to_skip); + + // Insert zero velocity to each point in the path. + for (auto & point : reference_path.points) { + point.point.longitudinal_velocity_mps = 0.0; + } + + output.path = reference_path; + output.reference_path = reference_path; + output.drivable_area_info.drivable_lanes = drivable_lanes; + + return output; +} + +} // namespace autoware::behavior_path_planner::utils diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp similarity index 96% rename from planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp index ecd02bfd63485..babcbefecbdeb 100644 --- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include -namespace behavior_path_planner::utils::traffic_light +namespace autoware::behavior_path_planner::utils::traffic_light { using motion_utils::calcSignedArcLength; @@ -147,4 +147,4 @@ bool isTrafficSignalStop( return false; } -} // namespace behavior_path_planner::utils::traffic_light +} // namespace autoware::behavior_path_planner::utils::traffic_light diff --git a/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp new file mode 100644 index 0000000000000..4610f7584ea29 --- /dev/null +++ b/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -0,0 +1,1656 @@ +// 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 "autoware_behavior_path_planner_common/utils/utils.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 + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace +{ +double calcInterpolatedZ( + const tier4_planning_msgs::msg::PathWithLaneId & input, + const geometry_msgs::msg::Point target_pos, const size_t seg_idx) +{ + const double closest_to_target_dist = motion_utils::calcSignedArcLength( + input.points, input.points.at(seg_idx).point.pose.position, + target_pos); // TODO(murooka) implement calcSignedArcLength(points, idx, point) + const double seg_dist = motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + + const double closest_z = input.points.at(seg_idx).point.pose.position.z; + const double next_z = input.points.at(seg_idx + 1).point.pose.position.z; + const double interpolated_z = + std::abs(seg_dist) < 1e-6 + ? next_z + : closest_z + (next_z - closest_z) * closest_to_target_dist / seg_dist; + return interpolated_z; +} + +double calcInterpolatedVelocity( + const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) +{ + const double seg_dist = motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); + + const double closest_vel = input.points.at(seg_idx).point.longitudinal_velocity_mps; + const double next_vel = input.points.at(seg_idx + 1).point.longitudinal_velocity_mps; + const double interpolated_vel = std::abs(seg_dist) < 1e-06 ? next_vel : closest_vel; + return interpolated_vel; +} +} // namespace + +namespace autoware::behavior_path_planner::utils +{ +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; +using geometry_msgs::msg::PoseWithCovarianceStamped; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; + +std::optional getPolygonByPoint( + const std::shared_ptr & route_handler, const lanelet::ConstPoint3d & point, + const std::string & polygon_name) +{ + const auto polygons = route_handler->getLaneletMapPtr()->polygonLayer.findUsages(point); + for (const auto & polygon : polygons) { + const std::string type = polygon.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == polygon_name) { + // NOTE: If there are multiple polygons on a point, only the front one is used. + return polygon; + } + } + return std::nullopt; +} + +double l2Norm(const Vector3 vector) +{ + return std::sqrt(std::pow(vector.x, 2) + std::pow(vector.y, 2) + std::pow(vector.z, 2)); +} + +double getDistanceBetweenPredictedPaths( + const PredictedPath & object_path, const PredictedPath & ego_path, const double start_time, + const double end_time, const double resolution) +{ + double min_distance = std::numeric_limits::max(); + for (double t = start_time; t < end_time; t += resolution) { + const auto object_pose = object_recognition_utils::calcInterpolatedPose(object_path, t); + if (!object_pose) { + continue; + } + const auto ego_pose = object_recognition_utils::calcInterpolatedPose(ego_path, t); + if (!ego_pose) { + continue; + } + double distance = tier4_autoware_utils::calcDistance3d(*object_pose, *ego_pose); + if (distance < min_distance) { + min_distance = distance; + } + } + return min_distance; +} + +double getDistanceBetweenPredictedPathAndObject( + const PredictedObject & object, const PredictedPath & ego_path, const double start_time, + const double end_time, const double resolution) +{ + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + auto t_delta{rclcpp::Duration::from_seconds(resolution)}; + double min_distance = std::numeric_limits::max(); + rclcpp::Time ros_start_time = clock.now() + rclcpp::Duration::from_seconds(start_time); + rclcpp::Time ros_end_time = clock.now() + rclcpp::Duration::from_seconds(end_time); + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + for (double t = start_time; t < end_time; t += resolution) { + const auto ego_pose = object_recognition_utils::calcInterpolatedPose(ego_path, t); + if (!ego_pose) { + continue; + } + Point2d ego_point{ego_pose->position.x, ego_pose->position.y}; + + double distance = boost::geometry::distance(obj_polygon, ego_point); + if (distance < min_distance) { + min_distance = distance; + } + } + return min_distance; +} + +bool checkCollisionBetweenPathFootprintsAndObjects( + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, + const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, const double margin) +{ + for (const auto & p : ego_path.points) { + if (checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, p.point.pose, dynamic_objects, margin)) { + return true; + } + } + return false; +} + +bool checkCollisionBetweenFootprintAndObjects( + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const PredictedObjects & dynamic_objects, const double margin) +{ + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); + + for (const auto & object : dynamic_objects.objects) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + const double distance = boost::geometry::distance(obj_polygon, vehicle_footprint); + if (distance < margin) return true; + } + return false; +} + +double calcLateralDistanceFromEgoToObject( + const Pose & ego_pose, const double vehicle_width, const PredictedObject & dynamic_object) +{ + double min_distance = std::numeric_limits::max(); + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(dynamic_object); + const auto vehicle_left_pose = + tier4_autoware_utils::calcOffsetPose(ego_pose, 0, vehicle_width / 2, 0); + const auto vehicle_right_pose = + tier4_autoware_utils::calcOffsetPose(ego_pose, 0, -vehicle_width / 2, 0); + + for (const auto & p : obj_polygon.outer()) { + const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + const double signed_distance_from_left = + tier4_autoware_utils::calcLateralDeviation(vehicle_left_pose, point); + const double signed_distance_from_right = + tier4_autoware_utils::calcLateralDeviation(vehicle_right_pose, point); + + if (signed_distance_from_left < 0.0 && signed_distance_from_right > 0.0) { + // point is between left and right + return 0.0; + } + + const double distance_from_ego = + std::min(std::abs(signed_distance_from_left), std::abs(signed_distance_from_right)); + min_distance = std::min(min_distance, distance_from_ego); + } + return min_distance; +} + +double calcLongitudinalDistanceFromEgoToObject( + const Pose & ego_pose, const double base_link2front, const double base_link2rear, + const PredictedObject & dynamic_object) +{ + double min_distance = std::numeric_limits::max(); + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(dynamic_object); + const auto vehicle_front_pose = + tier4_autoware_utils::calcOffsetPose(ego_pose, base_link2front, 0, 0); + const auto vehicle_rear_pose = + tier4_autoware_utils::calcOffsetPose(ego_pose, base_link2rear, 0, 0); + + for (const auto & p : obj_polygon.outer()) { + const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); + + // forward is positive + const double signed_distance_from_front = + tier4_autoware_utils::calcLongitudinalDeviation(vehicle_front_pose, point); + // backward is positive + const double signed_distance_from_rear = + -tier4_autoware_utils::calcLongitudinalDeviation(vehicle_rear_pose, point); + + if (signed_distance_from_front < 0.0 && signed_distance_from_rear < 0.0) { + // point is between front and rear + return 0.0; + } + + const double distance_from_ego = + std::min(std::abs(signed_distance_from_front), std::abs(signed_distance_from_rear)); + min_distance = std::min(min_distance, distance_from_ego); + } + return min_distance; +} + +double calcLongitudinalDistanceFromEgoToObjects( + const Pose & ego_pose, double base_link2front, double base_link2rear, + const PredictedObjects & dynamic_objects) +{ + double min_distance = std::numeric_limits::max(); + for (const auto & object : dynamic_objects.objects) { + min_distance = std::min( + min_distance, + calcLongitudinalDistanceFromEgoToObject(ego_pose, base_link2front, base_link2rear, object)); + } + return min_distance; +} + +std::vector calcObjectsDistanceToPath( + const PredictedObjects & objects, const PathWithLaneId & ego_path) +{ + std::vector distance_array; + for (const auto & obj : objects.objects) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); + LineString2d ego_path_line; + ego_path_line.reserve(ego_path.points.size()); + for (const auto & p : ego_path.points) { + boost::geometry::append( + ego_path_line, Point2d(p.point.pose.position.x, p.point.pose.position.y)); + } + const double distance = boost::geometry::distance(obj_polygon, ego_path_line); + distance_array.push_back(distance); + } + return distance_array; +} + +template +bool exists(std::vector vec, T element) +{ + return std::find(vec.begin(), vec.end(), element) != vec.end(); +} + +std::optional findIndexOutOfGoalSearchRange( + const std::vector & points, const Pose & goal, + const int64_t goal_lane_id, const double max_dist = std::numeric_limits::max()) +{ + if (points.empty()) { + return std::nullopt; + } + + // find goal index + size_t min_dist_index; + double min_dist = std::numeric_limits::max(); + { + bool found = false; + for (size_t i = 0; i < points.size(); ++i) { + const auto & lane_ids = points.at(i).lane_ids; + + const double dist_to_goal = + tier4_autoware_utils::calcDistance2d(points.at(i).point.pose, goal); + const bool is_goal_lane_id_in_point = + std::find(lane_ids.begin(), lane_ids.end(), goal_lane_id) != lane_ids.end(); + if (dist_to_goal < max_dist && dist_to_goal < min_dist && is_goal_lane_id_in_point) { + min_dist_index = i; + min_dist = dist_to_goal; + found = true; + } + } + if (!found) { + return std::nullopt; + } + } + + // find index out of goal search range + size_t min_dist_out_of_range_index = min_dist_index; + for (int i = min_dist_index; 0 <= i; --i) { + const double dist = tier4_autoware_utils::calcDistance2d(points.at(i).point, goal); + min_dist_out_of_range_index = i; + if (max_dist < dist) { + break; + } + } + + return min_dist_out_of_range_index; +} + +// goal does not have z +bool setGoal( + const double search_radius_range, [[maybe_unused]] const double search_rad_range, + const PathWithLaneId & input, const Pose & goal, const int64_t goal_lane_id, + PathWithLaneId * output_ptr) +{ + try { + if (input.points.empty()) { + return false; + } + + // calculate refined_goal with interpolation + // NOTE: goal does not have valid z, that will be calculated by interpolation here + PathPointWithLaneId refined_goal{}; + const size_t closest_seg_idx_for_goal = + findNearestSegmentIndex(input.points, goal, 3.0, M_PI_4); + refined_goal.point.pose = goal; + refined_goal.point.pose.position.z = + calcInterpolatedZ(input, goal.position, closest_seg_idx_for_goal); + refined_goal.point.longitudinal_velocity_mps = 0.0; + + // calculate pre_refined_goal with interpolation + // NOTE: z and velocity are filled + PathPointWithLaneId pre_refined_goal{}; + constexpr double goal_to_pre_goal_distance = -1.0; + pre_refined_goal.point.pose = + tier4_autoware_utils::calcOffsetPose(goal, goal_to_pre_goal_distance, 0.0, 0.0); + const size_t closest_seg_idx_for_pre_goal = + findNearestSegmentIndex(input.points, pre_refined_goal.point.pose, 3.0, M_PI_4); + pre_refined_goal.point.pose.position.z = + calcInterpolatedZ(input, pre_refined_goal.point.pose.position, closest_seg_idx_for_pre_goal); + pre_refined_goal.point.longitudinal_velocity_mps = + calcInterpolatedVelocity(input, closest_seg_idx_for_pre_goal); + + // find min_dist_out_of_circle_index whose distance to goal is longer than search_radius_range + const auto min_dist_out_of_circle_index_opt = + findIndexOutOfGoalSearchRange(input.points, goal, goal_lane_id, search_radius_range); + if (!min_dist_out_of_circle_index_opt) { + return false; + } + const size_t min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.value(); + + // create output points + output_ptr->points.reserve(output_ptr->points.size() + min_dist_out_of_circle_index + 3); + for (size_t i = 0; i <= min_dist_out_of_circle_index; ++i) { + output_ptr->points.push_back(input.points.at(i)); + } + output_ptr->points.push_back(pre_refined_goal); + output_ptr->points.push_back(refined_goal); + + { // fill skipped lane ids + // pre refined goal + auto & pre_goal = output_ptr->points.at(output_ptr->points.size() - 2); + for (size_t i = min_dist_out_of_circle_index + 1; i < input.points.size(); ++i) { + for (const auto target_lane_id : input.points.at(i).lane_ids) { + const bool is_lane_id_found = + std::find(pre_goal.lane_ids.begin(), pre_goal.lane_ids.end(), target_lane_id) != + pre_goal.lane_ids.end(); + if (!is_lane_id_found) { + pre_goal.lane_ids.push_back(target_lane_id); + } + } + } + + // goal + output_ptr->points.back().lane_ids = input.points.back().lane_ids; + } + + output_ptr->left_bound = input.left_bound; + output_ptr->right_bound = input.right_bound; + return true; + } catch (std::out_of_range & ex) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), + "failed to set goal: " << ex.what()); + return false; + } +} + +const Pose refineGoal(const Pose & goal, const lanelet::ConstLanelet & goal_lanelet) +{ + // return goal; + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal.position); + const double distance = boost::geometry::distance( + goal_lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(lanelet_point).basicPoint()); + if (distance < std::numeric_limits::epsilon()) { + return goal; + } + + const auto segment = lanelet::utils::getClosestSegment( + lanelet::utils::to2D(lanelet_point), goal_lanelet.centerline()); + if (segment.empty()) { + return goal; + } + + Pose refined_goal; + { + // find position + const auto p1 = segment.front().basicPoint(); + const auto p2 = segment.back().basicPoint(); + const auto direction_vector = (p2 - p1).normalized(); + const auto p1_to_goal = lanelet_point.basicPoint() - p1; + const double s = direction_vector.dot(p1_to_goal); + const auto refined_point = p1 + direction_vector * s; + + refined_goal.position.x = refined_point.x(); + refined_goal.position.y = refined_point.y(); + refined_goal.position.z = refined_point.z(); + + // find orientation + const double yaw = std::atan2(direction_vector.y(), direction_vector.x()); + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, yaw); + refined_goal.orientation = tf2::toMsg(tf_quat); + } + return refined_goal; +} + +PathWithLaneId refinePathForGoal( + const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, + const Pose & goal, const int64_t goal_lane_id) +{ + PathWithLaneId filtered_path = input; + PathWithLaneId path_with_goal; + filtered_path.points = motion_utils::removeOverlapPoints(filtered_path.points); + + // always set zero velocity at the end of path for safety + if (!filtered_path.points.empty()) { + filtered_path.points.back().point.longitudinal_velocity_mps = 0.0; + } + + if (setGoal( + search_radius_range, search_rad_range, filtered_path, goal, goal_lane_id, + &path_with_goal)) { + return path_with_goal; + } else { + return filtered_path; + } +} + +bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id) +{ + for (const auto & lane : lanes) { + if (lane.id() == goal_id) { + return true; + } + } + return false; +} + +bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes) +{ + for (const auto & lane : lanes) { + if (lanelet::utils::isInLanelet(pose, lane)) { + return true; + } + } + return false; +} + +bool isInLaneletWithYawThreshold( + const Pose & current_pose, const lanelet::ConstLanelet & lanelet, const double yaw_threshold, + const double radius) +{ + const double pose_yaw = tf2::getYaw(current_pose.orientation); + const double lanelet_angle = lanelet::utils::getLaneletAngle(lanelet, current_pose.position); + const double angle_diff = + std::abs(tier4_autoware_utils::normalizeRadian(lanelet_angle - pose_yaw)); + + return (angle_diff < std::abs(yaw_threshold)) && + lanelet::utils::isInLanelet(current_pose, lanelet, radius); +} + +bool isEgoOutOfRoute( + const Pose & self_pose, const std::optional & modified_goal, + const std::shared_ptr & route_handler) +{ + const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid()) + ? modified_goal->pose + : route_handler->getGoalPose(); + + lanelet::ConstLanelet goal_lane; + const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose); + if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front(); + const auto is_failed_getting_lanelet = + shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane); + if (is_failed_getting_lanelet) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet"); + return true; + } + + // If ego vehicle is over goal on goal lane, return true + const double yaw_threshold = tier4_autoware_utils::deg2rad(90); + if (isInLaneletWithYawThreshold(self_pose, goal_lane, yaw_threshold)) { + constexpr double buffer = 1.0; + const auto ego_arc_coord = lanelet::utils::getArcCoordinates({goal_lane}, self_pose); + const auto goal_arc_coord = + lanelet::utils::getArcCoordinates({goal_lane}, route_handler->getGoalPose()); + if (ego_arc_coord.length > goal_arc_coord.length + buffer) { + return true; + } else { + return false; + } + } + + // If ego vehicle is out of the closest lanelet, return true + // Check if ego vehicle is in shoulder lane + const bool is_in_shoulder_lane = !route_handler->getShoulderLaneletsAtPose(self_pose).empty(); + // Check if ego vehicle is in road lane + const bool is_in_road_lane = std::invoke([&]() { + lanelet::ConstLanelet closest_road_lane; + if (!route_handler->getClosestLaneletWithinRoute(self_pose, &closest_road_lane)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("util"), + "cannot find closest road lanelet"); + return false; + } + + if (lanelet::utils::isInLanelet(self_pose, closest_road_lane)) { + return true; + } + + // check previous lanes for backward driving (e.g. pull out) + const auto prev_lanes = route_handler->getPreviousLanelets(closest_road_lane); + for (const auto & lane : prev_lanes) { + if (lanelet::utils::isInLanelet(self_pose, lane)) { + return true; + } + } + + return false; + }); + if (!is_in_shoulder_lane && !is_in_road_lane) { + return true; + } + + return false; +} + +bool isEgoWithinOriginalLane( + const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin) +{ + const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); + const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); + const auto base_link2front = common_param.base_link2front; + const auto base_link2rear = common_param.base_link2rear; + const auto vehicle_width = common_param.vehicle_width; + const auto vehicle_poly = + tier4_autoware_utils::toFootprint(current_pose, base_link2front, base_link2rear, vehicle_width); + + // Check if the ego vehicle is entirely within the lane with a given outer margin. + for (const auto & p : vehicle_poly.outer()) { + // When the point is in the polygon, the distance is 0. When it is out of the polygon, return a + // positive value. + const auto dist = boost::geometry::distance(p, lanelet::utils::to2D(lane_poly).basicPolygon()); + if (dist > std::max(outer_margin, 0.0)) { + return false; // out of polygon + } + } + + return true; // inside polygon +} + +lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes) +{ + lanelet::ConstLanelets lanes; + + const auto has_same_lane = [&](const auto & 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(); + }; + + lanes.push_back(drivable_lanes.right_lane); + if (!has_same_lane(drivable_lanes.left_lane)) { + lanes.push_back(drivable_lanes.left_lane); + } + + for (const auto & ml : drivable_lanes.middle_lanes) { + if (!has_same_lane(ml)) { + lanes.push_back(ml); + } + } + + return lanes; +} + +lanelet::ConstLanelets transformToLanelets(const std::vector & drivable_lanes) +{ + lanelet::ConstLanelets lanes; + + for (const auto & drivable_lane : drivable_lanes) { + const auto transformed_lane = transformToLanelets(drivable_lane); + lanes.insert(lanes.end(), transformed_lane.begin(), transformed_lane.end()); + } + + return lanes; +} + +std::optional getRightLanelet( + const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes) +{ + for (const auto & shoulder_lane : shoulder_lanes) { + if (shoulder_lane.leftBound().id() == current_lane.rightBound().id()) { + return shoulder_lane; + } + } + + return {}; +} + +std::optional getLeftLanelet( + const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes) +{ + for (const auto & shoulder_lane : shoulder_lanes) { + if (shoulder_lane.rightBound().id() == current_lane.leftBound().id()) { + return shoulder_lane; + } + } + + return {}; +} + +// generate drivable area by expanding path for freespace +double getDistanceToEndOfLane(const Pose & current_pose, const lanelet::ConstLanelets & lanelets) +{ + const auto & arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); + const double lanelet_length = lanelet::utils::getLaneletLength3d(lanelets); + return lanelet_length - arc_coordinates.length; +} + +double getDistanceToNextIntersection( + const Pose & current_pose, const lanelet::ConstLanelets & lanelets) +{ + const auto & arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); + + lanelet::ConstLanelet current_lanelet; + if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { + return std::numeric_limits::max(); + } + + double distance = 0; + bool is_after_current_lanelet = false; + for (const auto & llt : lanelets) { + if (llt == current_lanelet) { + is_after_current_lanelet = true; + } + if (is_after_current_lanelet && llt.hasAttribute("turn_direction")) { + bool is_lane_change_yes = false; + const auto right_line = llt.rightBound(); + if (right_line.hasAttribute(lanelet::AttributeNamesString::LaneChange)) { + const auto attr = right_line.attribute(lanelet::AttributeNamesString::LaneChange); + if (attr.value() == std::string("yes")) { + is_lane_change_yes = true; + } + } + const auto left_line = llt.leftBound(); + if (left_line.hasAttribute(lanelet::AttributeNamesString::LaneChange)) { + const auto attr = left_line.attribute(lanelet::AttributeNamesString::LaneChange); + if (attr.value() == std::string("yes")) { + is_lane_change_yes = true; + } + } + if (!is_lane_change_yes) { + return distance - arc_coordinates.length; + } + } + distance += lanelet::utils::getLaneletLength3d(llt); + } + + return std::numeric_limits::max(); +} + +double getDistanceToCrosswalk( + const Pose & current_pose, const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphContainer & overall_graphs) +{ + const auto & arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); + + lanelet::ConstLanelet current_lanelet; + if (!lanelet::utils::query::getClosestLanelet(lanelets, current_pose, ¤t_lanelet)) { + return std::numeric_limits::infinity(); + } + + double distance = 0; + bool is_after_current_lanelet = false; + for (const auto & llt : lanelets) { + if (llt == current_lanelet) { + is_after_current_lanelet = true; + } + + if (is_after_current_lanelet) { + const auto conflicting_crosswalks = overall_graphs.conflictingInGraph(llt, 1); + if (!(conflicting_crosswalks.empty())) { + // create centerline + const lanelet::ConstLineString2d lanelet_centerline = llt.centerline2d(); + LineString2d centerline; + centerline.reserve(lanelet_centerline.size()); + for (const auto & point : lanelet_centerline) { + boost::geometry::append(centerline, Point2d(point.x(), point.y())); + } + + // create crosswalk polygon and calculate distance + double min_distance_to_crosswalk = std::numeric_limits::infinity(); + for (const auto & crosswalk : conflicting_crosswalks) { + lanelet::CompoundPolygon2d lanelet_crosswalk_polygon = crosswalk.polygon2d(); + Polygon2d polygon; + polygon.outer().reserve(lanelet_crosswalk_polygon.size() + 1); + for (const auto & point : lanelet_crosswalk_polygon) { + polygon.outer().emplace_back(point.x(), point.y()); + } + polygon.outer().push_back(polygon.outer().front()); + + std::vector points_intersection; + boost::geometry::intersection(centerline, polygon, points_intersection); + + for (const auto & point : points_intersection) { + lanelet::ConstLanelets lanelets = {llt}; + Pose pose_point; + pose_point.position.x = point.x(); + pose_point.position.y = point.y(); + const lanelet::ArcCoordinates & arc_crosswalk = + lanelet::utils::getArcCoordinates(lanelets, pose_point); + + const double distance_to_crosswalk = arc_crosswalk.length; + if (distance_to_crosswalk < min_distance_to_crosswalk) { + min_distance_to_crosswalk = distance_to_crosswalk; + } + } + } + if (distance + min_distance_to_crosswalk > arc_coordinates.length) { + return distance + min_distance_to_crosswalk - arc_coordinates.length; + } + } + } + distance += lanelet::utils::getLaneletLength3d(llt); + } + + return std::numeric_limits::infinity(); +} + +double getSignedDistance( + const Pose & current_pose, const Pose & goal_pose, const lanelet::ConstLanelets & lanelets) +{ + const auto arc_current = lanelet::utils::getArcCoordinates(lanelets, current_pose); + const auto arc_goal = lanelet::utils::getArcCoordinates(lanelets, goal_pose); + + return arc_goal.length - arc_current.length; +} + +std::vector getIds(const lanelet::ConstLanelets & lanelets) +{ + std::vector ids; + ids.reserve(lanelets.size()); + for (const auto & llt : lanelets) { + ids.push_back(llt.id()); + } + return ids; +} + +PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path) +{ + const size_t original_size = path.points.size(); + + // insert stop point + const auto insert_idx = motion_utils::insertStopPoint(length, path.points); + if (!insert_idx) { + return PathPointWithLaneId(); + } + + // check if a stop point is inserted + if (path.points.size() == original_size) { + return path.points.at(*insert_idx); + } + + if (*insert_idx == 0 || *insert_idx == original_size - 1) { + return path.points.at(*insert_idx); + } + + // check lane ids of the inserted stop point + path.points.at(*insert_idx).lane_ids = {}; + const auto & prev_lane_ids = path.points.at(*insert_idx - 1).lane_ids; + const auto & next_lane_ids = path.points.at(*insert_idx + 1).lane_ids; + + for (const auto target_lane_id : prev_lane_ids) { + if ( + std::find(next_lane_ids.begin(), next_lane_ids.end(), target_lane_id) != + next_lane_ids.end()) { + path.points.at(*insert_idx).lane_ids.push_back(target_lane_id); + } + } + + // If there is no lane ids, we are going to insert prev lane ids + if (path.points.at(*insert_idx).lane_ids.empty()) { + path.points.at(*insert_idx).lane_ids = prev_lane_ids; + } + + return path.points.at(*insert_idx); +} + +double getSignedDistanceFromBoundary( + const lanelet::ConstLanelets & lanelets, const Pose & pose, bool left_side) +{ + lanelet::ConstLanelet closest_lanelet; + lanelet::ArcCoordinates arc_coordinates; + if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); + const auto & boundary_line_2d = left_side + ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) + : lanelet::utils::to2D(closest_lanelet.rightBound3d()); + arc_coordinates = lanelet::geometry::toArcCoordinates( + boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + } else { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), + "closest shoulder lanelet not found."); + } + + return arc_coordinates.distance; +} + +std::optional getSignedDistanceFromBoundary( + 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) -> std::optional> { + Pose vehicle_corner_pose{}; + vehicle_corner_pose.position = vehicle_corner_point; + vehicle_corner_pose.orientation = vehicle_pose.orientation; + + std::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 std::nullopt; + }; + + // Calculate the lateral distance for both the rear and front corners of the vehicle. + const std::optional> rear_lateral_distance_with_idx = + calcLateralDistanceFromBound(rear_corner_point); + const std::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 {}; + } + // 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.value().first; + } else if (!front_lateral_distance_with_idx) { + return rear_lateral_distance_with_idx.value().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.value().first, + front_lateral_distance_with_idx.value().first) + : std::min( + rear_lateral_distance_with_idx.value().first, + front_lateral_distance_with_idx.value().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.value().second + 1; + i < front_lateral_distance_with_idx.value().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( + const lanelet::ConstLanelets & lanelet_sequence, const lanelet::ConstLanelet & target_lane, + const Pose & pose) +{ + const auto arc_pose = lanelet::utils::getArcCoordinates(lanelet_sequence, pose); + + const auto target_center_line = target_lane.centerline().basicLineString(); + + Pose front_pose, back_pose; + + { + const auto front_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.front()); + const double front_yaw = lanelet::utils::getLaneletAngle(target_lane, front_point); + front_pose.position = front_point; + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, front_yaw); + front_pose.orientation = tf2::toMsg(tf_quat); + } + + { + const auto back_point = lanelet::utils::conversion::toGeomMsgPt(target_center_line.back()); + const double back_yaw = lanelet::utils::getLaneletAngle(target_lane, back_point); + back_pose.position = back_point; + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, back_yaw); + back_pose.orientation = tf2::toMsg(tf_quat); + } + + const auto arc_front = lanelet::utils::getArcCoordinates(lanelet_sequence, front_pose); + const auto arc_back = lanelet::utils::getArcCoordinates(lanelet_sequence, back_pose); + + return std::max( + 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); +} + +Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) +{ + Polygon2d ret; + for (const auto & p : polygon) { + ret.outer().emplace_back(p.x(), p.y()); + } + ret.outer().push_back(ret.outer().front()); + + return tier4_autoware_utils::isClockwise(ret) ? ret : tier4_autoware_utils::inverseClockwise(ret); +} + +std::vector getTargetLaneletPolygons( + const lanelet::PolygonLayer & map_polygons, lanelet::ConstLanelets & lanelets, const Pose & pose, + const double check_length, const std::string & target_type) +{ + std::vector polygons; + + // create lanelet polygon + const auto arclength = lanelet::utils::getArcCoordinates(lanelets, pose); + const auto llt_polygon = lanelet::utils::getPolygonFromArcLength( + lanelets, arclength.length, arclength.length + check_length); + const auto llt_polygon_2d = lanelet::utils::to2D(llt_polygon).basicPolygon(); + + // If the number of vertices is not enough to create polygon, return empty polygon container + if (llt_polygon_2d.size() < 3) { + return polygons; + } + + Polygon2d llt_polygon_bg; + llt_polygon_bg.outer().reserve(llt_polygon_2d.size() + 1); + for (const auto & llt_pt : llt_polygon_2d) { + llt_polygon_bg.outer().emplace_back(llt_pt.x(), llt_pt.y()); + } + llt_polygon_bg.outer().push_back(llt_polygon_bg.outer().front()); + + for (const auto & map_polygon : map_polygons) { + const std::string type = map_polygon.attributeOr(lanelet::AttributeName::Type, ""); + // If the target_type is different + // or the number of vertices is not enough to create polygon, skip the loop + if (type == target_type && map_polygon.size() > 2) { + // create map polygon + Polygon2d map_polygon_bg; + map_polygon_bg.outer().reserve(map_polygon.size() + 1); + for (const auto & pt : map_polygon) { + map_polygon_bg.outer().emplace_back(pt.x(), pt.y()); + } + map_polygon_bg.outer().push_back(map_polygon_bg.outer().front()); + if (boost::geometry::intersects(llt_polygon_bg, map_polygon_bg)) { + polygons.push_back(map_polygon_bg); + } + } + } + return polygons; +} + +// TODO(Horibe) There is a similar function in route_handler. +std::shared_ptr generateCenterLinePath( + const std::shared_ptr & planner_data) +{ + auto centerline_path = std::make_shared(); + + const auto & p = planner_data->parameters; + + const auto & route_handler = planner_data->route_handler; + const auto & pose = planner_data->self_odometry->pose.pose; + + lanelet::ConstLanelet current_lane; + if (!route_handler->getClosestLaneletWithinRoute(pose, ¤t_lane)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), + "failed to find closest lanelet within route!!!"); + return {}; // TODO(Horibe) What should be returned? + } + + // For lanelet_sequence with desired length + lanelet::ConstLanelets lanelet_sequence = route_handler->getLaneletSequence( + current_lane, pose, p.backward_path_length, p.forward_path_length); + + std::vector drivable_lanes(lanelet_sequence.size()); + for (size_t i = 0; i < lanelet_sequence.size(); ++i) { + drivable_lanes.at(i).left_lane = lanelet_sequence.at(i); + drivable_lanes.at(i).right_lane = lanelet_sequence.at(i); + } + + *centerline_path = getCenterLinePath( + *route_handler, lanelet_sequence, pose, p.backward_path_length, p.forward_path_length, p); + + centerline_path->header = route_handler->getRouteHeader(); + + return centerline_path; +} + +PathWithLaneId getCenterLinePathFromLanelet( + const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data) +{ + const auto & route_handler = planner_data->route_handler; + const auto & current_pose = planner_data->self_odometry->pose.pose; + const auto & p = planner_data->parameters; + + const auto reference_lanes = route_handler->getLaneletSequence( + lanelet, current_pose, p.backward_path_length, p.forward_path_length); + + return getCenterLinePath( + *route_handler, reference_lanes, current_pose, p.backward_path_length, p.forward_path_length, + p); +} + +PathWithLaneId getCenterLinePath( + const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelet_sequence, + const Pose & pose, const double backward_path_length, const double forward_path_length, + const BehaviorPathPlannerParameters & parameter) +{ + PathWithLaneId reference_path; + + if (lanelet_sequence.empty()) { + return reference_path; + } + + const auto arc_coordinates = lanelet::utils::getArcCoordinates(lanelet_sequence, pose); + const double s = arc_coordinates.length; + const double s_backward = std::max(0., s - backward_path_length); + double s_forward = s + forward_path_length; + + if (route_handler.isDeadEndLanelet(lanelet_sequence.back())) { + const auto lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); + s_forward = std::clamp(s_forward, 0.0, lane_length); + } + + if (route_handler.isInGoalRouteSection(lanelet_sequence.back())) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(lanelet_sequence, route_handler.getGoalPose()); + s_forward = std::clamp(s_forward, 0.0, goal_arc_coordinates.length); + } + + const auto raw_path_with_lane_id = + route_handler.getCenterLinePath(lanelet_sequence, s_backward, s_forward, true); + auto resampled_path_with_lane_id = motion_utils::resamplePath( + raw_path_with_lane_id, parameter.input_path_interval, parameter.enable_akima_spline_first); + + // convert centerline, which we consider as CoG center, to rear wheel center + if (parameter.enable_cog_on_centerline) { + const double rear_to_cog = parameter.vehicle_length / 2 - parameter.rear_overhang; + return motion_utils::convertToRearWheelCenter(resampled_path_with_lane_id, rear_to_cog); + } + + return resampled_path_with_lane_id; +} + +// for lane following +PathWithLaneId setDecelerationVelocity( + const RouteHandler & route_handler, const PathWithLaneId & input, + const lanelet::ConstLanelets & lanelet_sequence, const double lane_change_prepare_duration, + const double lane_change_buffer) +{ + auto reference_path = input; + if ( + route_handler.isDeadEndLanelet(lanelet_sequence.back()) && + lane_change_prepare_duration > std::numeric_limits::epsilon()) { + for (auto & point : reference_path.points) { + const double lane_length = lanelet::utils::getLaneletLength2d(lanelet_sequence); + const auto arclength = lanelet::utils::getArcCoordinates(lanelet_sequence, point.point.pose); + const double distance_to_end = + std::max(0.0, lane_length - std::abs(lane_change_buffer) - arclength.length); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast(distance_to_end / lane_change_prepare_duration)); + } + } + return reference_path; +} + +// TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex inside the +// function +PathWithLaneId setDecelerationVelocity( + const PathWithLaneId & input, const double target_velocity, const Pose target_pose, + const double buffer, const double deceleration_interval) +{ + auto reference_path = input; + + for (auto & point : reference_path.points) { + const auto arclength_to_target = std::max( + 0.0, motion_utils::calcSignedArcLength( + reference_path.points, point.point.pose.position, target_pose.position) + + buffer); + if (arclength_to_target > deceleration_interval) continue; + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast( + (arclength_to_target / deceleration_interval) * + (point.point.longitudinal_velocity_mps - target_velocity) + + target_velocity)); + } + + const auto stop_point_length = + motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + buffer; + constexpr double eps{0.01}; + if (std::abs(target_velocity) < eps && stop_point_length > 0.0) { + const auto stop_point = utils::insertStopPoint(stop_point_length, reference_path); + } + + return reference_path; +} + +std::uint8_t getHighestProbLabel(const std::vector & classification) +{ + std::uint8_t label = ObjectClassification::UNKNOWN; + float highest_prob = 0.0; + for (const auto & _class : classification) { + if (highest_prob < _class.probability) { + highest_prob = _class.probability; + label = _class.label; + } + } + return label; +} + +lanelet::ConstLanelets getCurrentLanes( + const std::shared_ptr & planner_data, const double backward_path_length, + const double forward_path_length) +{ + const auto & route_handler = planner_data->route_handler; + const auto current_pose = planner_data->self_odometry->pose.pose; + + lanelet::ConstLanelet current_lane; + if (!route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), clock, 1000, + "failed to find closest lanelet within route!!!"); + return {}; // TODO(Horibe) what should be returned? + } + + // For current_lanes with desired length + return route_handler->getLaneletSequence( + current_lane, current_pose, backward_path_length, forward_path_length); +} + +lanelet::ConstLanelets getCurrentLanes(const std::shared_ptr & planner_data) +{ + const auto & common_parameters = planner_data->parameters; + return getCurrentLanes( + planner_data, common_parameters.backward_path_length, common_parameters.forward_path_length); +} + +lanelet::ConstLanelets getCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data) +{ + const auto & route_handler = planner_data->route_handler; + const auto & current_pose = planner_data->self_odometry->pose.pose; + const auto & p = planner_data->parameters; + + std::set lane_ids; + for (const auto & p : path.points) { + for (const auto & id : p.lane_ids) { + lane_ids.insert(id); + } + } + + lanelet::ConstLanelets reference_lanes{}; + for (const auto & id : lane_ids) { + reference_lanes.push_back(planner_data->route_handler->getLaneletsFromId(id)); + } + + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet(reference_lanes, current_pose, ¤t_lane); + 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' + // if the extended prior lanes is in same lane sequence with current lanes + 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(); + }); + }; + auto extended_lanes = current_lanes; + while (rclcpp::ok()) { + const size_t pre_extension_size = extended_lanes.size(); // Get existing size before extension + extended_lanes = extendPrevLane(route_handler, extended_lanes, true); + if (extended_lanes.size() == pre_extension_size) break; + if (have_front_lanes(extended_lanes)) { + current_lanes = extended_lanes; + break; + } + } + + return current_lanes; +} + +lanelet::ConstLanelets extendNextLane( + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route) +{ + if (lanes.empty()) return lanes; + + auto extended_lanes = lanes; + + // Add next lane + const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back()); + if (!next_lanes.empty()) { + std::optional target_next_lane; + if (!only_in_route) { + target_next_lane = next_lanes.front(); + } + // use the next lane in route if it exists + for (const auto & next_lane : next_lanes) { + if (route_handler->isRouteLanelet(next_lane)) { + target_next_lane = next_lane; + } + } + if (target_next_lane) { + extended_lanes.push_back(*target_next_lane); + } + } + + return extended_lanes; +} + +lanelet::ConstLanelets extendPrevLane( + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route) +{ + if (lanes.empty()) return lanes; + + auto extended_lanes = lanes; + + // Add previous lane + const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front()); + if (!prev_lanes.empty()) { + std::optional target_prev_lane; + if (!only_in_route) { + target_prev_lane = prev_lanes.front(); + } + // use the previous lane in route if it exists + for (const auto & prev_lane : prev_lanes) { + if (route_handler->isRouteLanelet(prev_lane)) { + target_prev_lane = prev_lane; + } + } + if (target_prev_lane) { + extended_lanes.insert(extended_lanes.begin(), *target_prev_lane); + } + } + return extended_lanes; +} + +lanelet::ConstLanelets extendLanes( + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) +{ + auto extended_lanes = extendNextLane(route_handler, lanes); + extended_lanes = extendPrevLane(route_handler, extended_lanes); + + return extended_lanes; +} + +lanelet::ConstLanelets getExtendedCurrentLanes( + const std::shared_ptr & planner_data, const double backward_length, + const double forward_length, const bool forward_only_in_route) +{ + auto lanes = getCurrentLanes(planner_data); + if (lanes.empty()) return lanes; + const auto start_lane = lanes.front(); + + double forward_length_sum = 0.0; + double backward_length_sum = 0.0; + + while (backward_length_sum < backward_length) { + auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for backward_length, + // the extending process will not finish. + if (extended_lanes.front().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + backward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.front()); + } else { + break; // no more previous lanes to add + } + lanes = extended_lanes; + } + + while (forward_length_sum < forward_length) { + auto extended_lanes = extendNextLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for forward_length, + // the extending process will not finish. + if (extended_lanes.back().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + forward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.back()); + } else { + break; // no more next lanes to add + } + + // stop extending when the lane outside of the route is reached + // if forward_length is a very large value, set it to true, + // as it may continue to extend forever. + if (forward_only_in_route) { + if (!planner_data->route_handler->isRouteLanelet(extended_lanes.back())) { + return lanes; + } + } + + lanes = extended_lanes; + } + + return lanes; +} + +lanelet::ConstLanelets getExtendedCurrentLanes( + const std::shared_ptr & planner_data) +{ + return extendLanes(planner_data->route_handler, getCurrentLanes(planner_data)); +} + +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route) +{ + auto lanes = getCurrentLanesFromPath(path, planner_data); + + if (lanes.empty()) return lanes; + const auto start_lane = lanes.front(); + double forward_length_sum = 0.0; + double backward_length_sum = 0.0; + + while (backward_length_sum < backward_length) { + auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for backward_length, + // the extending process will not finish. + if (extended_lanes.front().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + backward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.front()); + } else { + break; // no more previous lanes to add + } + lanes = extended_lanes; + } + + while (forward_length_sum < forward_length) { + auto extended_lanes = extendNextLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for forward_length, + // the extending process will not finish. + if (extended_lanes.back().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + forward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.back()); + } else { + break; // no more next lanes to add + } + + // stop extending when the lane outside of the route is reached + // if forward_length is a very large value, set it to true, + // as it may continue to extend forever. + if (forward_only_in_route) { + if (!planner_data->route_handler->isRouteLanelet(extended_lanes.back())) { + return lanes; + } + } + + lanes = extended_lanes; + } + + return lanes; +} + +std::vector getPrecedingLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length) +{ + if (target_lanes.empty()) { + return {}; + } + + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); + + if (arc_length.length >= backward_length) { + return {}; + } + + const auto & front_lane = target_lanes.front(); + return route_handler.getPrecedingLaneletSequence( + front_lane, std::abs(backward_length - arc_length.length), {front_lane}); +} + +lanelet::ConstLanelets getBackwardLanelets( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const double backward_length) +{ + const auto preceding_lanes = + getPrecedingLanelets(route_handler, target_lanes, current_pose, backward_length); + const auto calc_sum = [](size_t sum, const auto & lanes) { return sum + lanes.size(); }; + const auto num_of_lanes = + std::accumulate(preceding_lanes.begin(), preceding_lanes.end(), 0u, calc_sum); + + lanelet::ConstLanelets backward_lanes{}; + backward_lanes.reserve(num_of_lanes); + + for (const auto & lanes : preceding_lanes) { + backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end()); + } + + return backward_lanes; +} + +lanelet::ConstLanelets calcLaneAroundPose( + const std::shared_ptr route_handler, const Pose & pose, const double forward_length, + const double backward_length, const double dist_threshold, const double yaw_threshold) +{ + lanelet::ConstLanelet current_lane; + if (!route_handler->getClosestLaneletWithConstrainsWithinRoute( + pose, ¤t_lane, dist_threshold, yaw_threshold)) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "failed to find closest lanelet within route!!!"); + return {}; // TODO(Horibe) + } + + // For current_lanes with desired length + lanelet::ConstLanelets current_lanes = + route_handler->getLaneletSequence(current_lane, pose, backward_length, forward_length); + + return current_lanes; +} + +bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold) +{ + // We need at least three points to compute relative angle + constexpr size_t relative_angle_points_num = 3; + if (path.points.size() < relative_angle_points_num) { + return true; + } + + for (size_t p1_id = 0; p1_id <= path.points.size() - relative_angle_points_num; ++p1_id) { + // Get Point1 + const auto & p1 = path.points.at(p1_id).point.pose.position; + + // Get Point2 + const auto & p2 = path.points.at(p1_id + 1).point.pose.position; + + // Get Point3 + const auto & p3 = path.points.at(p1_id + 2).point.pose.position; + + // ignore invert driving direction + if ( + path.points.at(p1_id).point.longitudinal_velocity_mps < 0 || + path.points.at(p1_id + 1).point.longitudinal_velocity_mps < 0 || + path.points.at(p1_id + 2).point.longitudinal_velocity_mps < 0) { + continue; + } + + // convert to p1 coordinate + const double x3 = p3.x - p1.x; + const double x2 = p2.x - p1.x; + const double y3 = p3.y - p1.y; + const double y2 = p2.y - p1.y; + + // calculate relative angle of vector p3 based on p1p2 vector + const double th = std::atan2(y2, x2); + const double th2 = + std::atan2(-x3 * std::sin(th) + y3 * std::cos(th), x3 * std::cos(th) + y3 * std::sin(th)); + if (std::abs(th2) > angle_threshold) { + // invalid angle + return false; + } + } + + return true; +} + +lanelet::ConstLanelets getLaneletsFromPath( + const PathWithLaneId & path, const std::shared_ptr & route_handler) +{ + std::vector unique_lanelet_ids; + for (const auto & p : path.points) { + const auto & lane_ids = p.lane_ids; + for (const auto & lane_id : lane_ids) { + if ( + std::find(unique_lanelet_ids.begin(), unique_lanelet_ids.end(), lane_id) == + unique_lanelet_ids.end()) { + unique_lanelet_ids.push_back(lane_id); + } + } + } + + lanelet::ConstLanelets lanelets; + for (const auto & lane_id : unique_lanelet_ids) { + lanelets.push_back(route_handler->getLaneletsFromId(lane_id)); + } + + return lanelets; +} + +std::string convertToSnakeCase(const std::string & input_str) +{ + std::string output_str = std::string{static_cast(std::tolower(input_str.at(0)))}; + for (size_t i = 1; i < input_str.length(); ++i) { + const auto input_chr = input_str.at(i); + if (std::isupper(input_chr)) { + output_str += "_" + std::string{static_cast(std::tolower(input_chr))}; + } else { + output_str += input_chr; + } + } + return output_str; +} + +bool isAllowedGoalModification(const std::shared_ptr & route_handler) +{ + return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); +} + +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) +{ + const Pose & goal_pose = route_handler->getOriginalGoalPose(); + return !route_handler->getShoulderLaneletsAtPose(goal_pose).empty(); +} +} // namespace autoware::behavior_path_planner::utils diff --git a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp b/planning/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp similarity index 93% rename from planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp rename to planning/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp index f244845679cea..0076aa6a1a669 100644 --- a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp +++ b/planning/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include @@ -179,7 +179,7 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area) autoware_map_msgs::msg::LaneletMapBin map; lanelet::LaneletMapPtr empty_lanelet_map_ptr = std::make_shared(); lanelet::utils::conversion::toBinMsg(empty_lanelet_map_ptr, &map); - route_handler::RouteHandler route_handler(map); + autoware::route_handler::RouteHandler route_handler(map); lanelet::ConstLanelets path_lanes = {}; drivable_area_expansion::PathWithLaneId path; { // Simple path with Y=0 and X = 0, 1, 2 @@ -223,14 +223,15 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area) params.vehicle_info.wheel_base_m = 2.0; params.vehicle_info.vehicle_width_m = 2.0; } - behavior_path_planner::PlannerData planner_data; + autoware::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); + planner_data.route_handler = + std::make_shared(route_handler); drivable_area_expansion::expand_drivable_area( - path, std::make_shared(planner_data)); + 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) { @@ -259,7 +260,7 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area) path.points[1].point.pose.position.y = 0.5; drivable_area_expansion::expand_drivable_area( - path, std::make_shared(planner_data)); + 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); diff --git a/planning/behavior_path_planner_common/test/test_safety_check.cpp b/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp similarity index 89% rename from planning/behavior_path_planner_common/test/test_safety_check.cpp rename to planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp index e2dd1ade110d0..2a3bbff492dfb 100644 --- a/planning/behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include @@ -27,8 +27,8 @@ constexpr double epsilon = 1e-6; +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using autoware_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; @@ -37,9 +37,9 @@ using tier4_autoware_utils::Polygon2d; TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { - using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; + using autoware::behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; - vehicle_info_util::VehicleInfo vehicle_info; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; vehicle_info.max_longitudinal_offset_m = 4.0; vehicle_info.vehicle_width_m = 2.0; vehicle_info.rear_overhang_m = 1.0; @@ -133,7 +133,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) { - using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; + using autoware::behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromYaw; @@ -183,8 +183,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) { - using behavior_path_planner::utils::path_safety_checker::calcRssDistance; - using behavior_path_planner::utils::path_safety_checker::RSSparams; + using autoware::behavior_path_planner::utils::path_safety_checker::calcRssDistance; + using autoware::behavior_path_planner::utils::path_safety_checker::RSSparams; { const double front_vel = 5.0; diff --git a/planning/behavior_path_planner_common/test/test_turn_signal.cpp b/planning/autoware_behavior_path_planner_common/test/test_turn_signal.cpp similarity index 98% rename from planning/behavior_path_planner_common/test/test_turn_signal.cpp rename to planning/autoware_behavior_path_planner_common/test/test_turn_signal.cpp index 2a8ea55f8d3c3..74c78e7024b72 100644 --- a/planning/behavior_path_planner_common/test/test_turn_signal.cpp +++ b/planning/autoware_behavior_path_planner_common/test/test_turn_signal.cpp @@ -11,7 +11,7 @@ // 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_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -22,13 +22,13 @@ #include #include +using autoware::behavior_path_planner::PathWithLaneId; +using autoware::behavior_path_planner::Pose; +using autoware::behavior_path_planner::TurnSignalDecider; +using autoware::behavior_path_planner::TurnSignalInfo; using autoware_planning_msgs::msg::PathPoint; using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; -using behavior_path_planner::PathWithLaneId; -using behavior_path_planner::Pose; -using behavior_path_planner::TurnSignalDecider; -using behavior_path_planner::TurnSignalInfo; using geometry_msgs::msg::Point; using geometry_msgs::msg::Twist; using tier4_autoware_utils::createPoint; diff --git a/planning/autoware_behavior_path_sampling_planner_module/CMakeLists.txt b/planning/autoware_behavior_path_sampling_planner_module/CMakeLists.txt new file mode 100644 index 0000000000000..2bab2904cc1e0 --- /dev/null +++ b/planning/autoware_behavior_path_sampling_planner_module/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_path_sampling_planner_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/sampling_planner_module.cpp + src/manager.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_sampling_planner_module/README.md b/planning/autoware_behavior_path_sampling_planner_module/README.md similarity index 100% rename from planning/behavior_path_sampling_planner_module/README.md rename to planning/autoware_behavior_path_sampling_planner_module/README.md diff --git a/planning/behavior_path_sampling_planner_module/config/sampling_planner.param.yaml b/planning/autoware_behavior_path_sampling_planner_module/config/sampling_planner.param.yaml similarity index 100% rename from planning/behavior_path_sampling_planner_module/config/sampling_planner.param.yaml rename to planning/autoware_behavior_path_sampling_planner_module/config/sampling_planner.param.yaml diff --git a/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/manager.hpp b/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/manager.hpp new file mode 100644 index 0000000000000..180d1af6190d2 --- /dev/null +++ b/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/manager.hpp @@ -0,0 +1,53 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ + +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_behavior_path_sampling_planner_module/sampling_planner_module.hpp" +#include "autoware_behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +class SamplingPlannerModuleManager : public SceneModuleManagerInterface +{ +public: + SamplingPlannerModuleManager() : SceneModuleManagerInterface{"sampling_planner"} {} + void init(rclcpp::Node * node) override; + + std::unique_ptr createNewSceneModuleInstance() override + { + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); + } + + void updateModuleParams( + [[maybe_unused]] const std::vector & parameters) override; + +private: + std::shared_ptr parameters_; +}; + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_module.hpp similarity index 81% rename from planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp rename to planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_module.hpp index 1683d4926c71b..9f518d102257b 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -12,34 +12,34 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ -#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ - -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" -#include "behavior_path_sampling_planner_module/util.hpp" -#include "bezier_sampler/bezier_sampling.hpp" -#include "frenet_planner/frenet_planner.hpp" +#ifndef AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ + +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" +#include "autoware_behavior_path_sampling_planner_module/util.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_frenet_planner/frenet_planner.hpp" +#include "autoware_sampler_common/constraints/footprint.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/constraints/soft_constraint.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "rclcpp/rclcpp.hpp" -#include "sampler_common/constraints/footprint.hpp" -#include "sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/constraints/soft_constraint.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/constants.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include "tier4_planning_msgs/msg/lateral_offset.hpp" #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" @@ -56,7 +56,7 @@ #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using autoware_planning_msgs::msg::TrajectoryPoint; struct SamplingPlannerData @@ -73,7 +73,7 @@ struct SamplingPlannerData struct SamplingPlannerDebugData { - std::vector sampled_candidates{}; + std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; std::vector obstacles{}; std::vector footprints{}; @@ -142,8 +142,8 @@ class SamplingPlannerModule : public SceneModuleInterface } SamplingPlannerDebugData debug_data_; - behavior_path_planner::HardConstraintsFunctionVector hard_constraints_; - behavior_path_planner::SoftConstraintsFunctionVector soft_constraints_; + HardConstraintsFunctionVector hard_constraints_; + SoftConstraintsFunctionVector soft_constraints_; private: SamplingPlannerData createPlannerData( @@ -231,25 +231,25 @@ class SamplingPlannerModule : public SceneModuleInterface void updateDebugMarkers(); void prepareConstraints( - sampler_common::Constraints & constraints, + autoware::sampler_common::Constraints & constraints, const PredictedObjects::ConstSharedPtr & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) const; - frenet_planner::SamplingParameters prepareSamplingParameters( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, + autoware::frenet_planner::SamplingParameters prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const SamplingPlannerInternalParameters & internal_params_); PathWithLaneId convertFrenetPathToPathWithLaneID( - const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, + const autoware::frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, const double path_z); // member // std::shared_ptr params_; std::shared_ptr internal_params_; - vehicle_info_util::VehicleInfo vehicle_info_{}; - std::optional prev_sampling_path_ = std::nullopt; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; + std::optional prev_sampling_path_ = std::nullopt; // move to utils void extendOutputDrivableArea( @@ -261,6 +261,6 @@ class SamplingPlannerModule : public SceneModuleInterface const std::shared_ptr & planner_data) const; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_parameters.hpp similarity index 78% rename from planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp rename to planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_parameters.hpp index 6e00fefe89574..6344af7163577 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp +++ b/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/sampling_planner_parameters.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ -#include "bezier_sampler/bezier_sampling.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_sampler_common/structures.hpp" #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::MultiPoint2d; @@ -78,14 +78,14 @@ struct Sampling std::vector target_lateral_positions{}; int nb_target_lateral_positions{}; Frenet frenet; - bezier_sampler::SamplingParameters bezier{}; + autoware::bezier_sampler::SamplingParameters bezier{}; }; struct SamplingPlannerInternalParameters { - sampler_common::Constraints constraints; + autoware::sampler_common::Constraints constraints; Sampling sampling; Preprocessing preprocessing{}; }; -} // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ diff --git a/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/util.hpp b/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/util.hpp new file mode 100644 index 0000000000000..1f910812f5360 --- /dev/null +++ b/planning/autoware_behavior_path_sampling_planner_module/include/autoware_behavior_path_sampling_planner_module/util.hpp @@ -0,0 +1,105 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +namespace autoware::behavior_path_planner +{ +using geometry_msgs::msg::Pose; + +struct SoftConstraintsInputs +{ + Pose goal_pose; + Pose ego_pose; + lanelet::ArcCoordinates ego_arc; + lanelet::ArcCoordinates goal_arc; + lanelet::ConstLanelets closest_lanelets_to_goal; + PlanResult reference_path; + PlanResult prev_module_path; + std::optional prev_path; + lanelet::ConstLanelets current_lanes; +}; + +using SoftConstraintsFunctionVector = std::vector>; + +using HardConstraintsFunctionVector = std::vector>; + +inline std::vector evaluateSoftConstraints( + autoware::sampler_common::Path & path, const autoware::sampler_common::Constraints & constraints, + const SoftConstraintsFunctionVector & soft_constraints_functions, + const SoftConstraintsInputs & input_data) +{ + std::vector constraints_results; + for (const auto & f : soft_constraints_functions) { + const auto cost = f(path, constraints, input_data); + constraints_results.push_back(cost); + } + if (constraints.soft.weights.size() != constraints_results.size()) { + path.cost = std::accumulate(constraints_results.begin(), constraints_results.end(), 0.0); + return constraints_results; + } + + path.cost = std::inner_product( + constraints_results.begin(), constraints_results.end(), constraints.soft.weights.begin(), 0.0); + return constraints_results; +} + +inline std::vector evaluateHardConstraints( + autoware::sampler_common::Path & path, const autoware::sampler_common::Constraints & constraints, + const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints) +{ + std::vector constraints_results; + bool constraints_passed = true; + for (const auto & f : hard_constraints) { + const bool constraint_check = f(path, constraints, footprint); + constraints_passed &= constraint_check; + constraints_results.push_back(constraint_check); + } + + path.constraints_satisfied = constraints_passed; + return constraints_results; +} + +inline autoware::sampler_common::State getInitialState( + const geometry_msgs::msg::Pose & pose, + const autoware::sampler_common::transform::Spline2D & reference_spline) +{ + autoware::sampler_common::State initial_state; + Point2d initial_state_pose{pose.position.x, pose.position.y}; + const auto rpy = tier4_autoware_utils::getRPY(pose.orientation); + initial_state.pose = initial_state_pose; + initial_state.frenet = reference_spline.frenet({pose.position.x, pose.position.y}); + initial_state.heading = rpy.z; + return initial_state; +} + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/autoware_behavior_path_sampling_planner_module/package.xml b/planning/autoware_behavior_path_sampling_planner_module/package.xml new file mode 100644 index 0000000000000..7934c2fb0ae91 --- /dev/null +++ b/planning/autoware_behavior_path_sampling_planner_module/package.xml @@ -0,0 +1,50 @@ + + + + autoware_behavior_path_sampling_planner_module + 0.1.0 + The autoware_behavior_path_sampling_planner_module package + + Daniel Sanchez + Maxime Clement + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_behavior_path_planner_common + autoware_bezier_sampler + autoware_frenet_planner + autoware_path_sampler + autoware_perception_msgs + autoware_planning_msgs + autoware_planning_test_manager + autoware_route_handler + autoware_vehicle_info_utils + autoware_vehicle_msgs + geometry_msgs + interpolation + lanelet2_extension + motion_utils + pluginlib + rclcpp + rclcpp_components + sensor_msgs + signal_processing + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_path_sampling_planner_module/plugins.xml b/planning/autoware_behavior_path_sampling_planner_module/plugins.xml new file mode 100644 index 0000000000000..ff121609ab0c3 --- /dev/null +++ b/planning/autoware_behavior_path_sampling_planner_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/autoware_behavior_path_sampling_planner_module/src/manager.cpp b/planning/autoware_behavior_path_sampling_planner_module/src/manager.cpp new file mode 100644 index 0000000000000..89be13bc9dabd --- /dev/null +++ b/planning/autoware_behavior_path_sampling_planner_module/src/manager.cpp @@ -0,0 +1,142 @@ +// Copyright 2024 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 "autoware_behavior_path_sampling_planner_module/manager.hpp" + +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ + +void SamplingPlannerModuleManager::init(rclcpp::Node * node) +{ + // init manager interface + initInterface(node, {""}); + + SamplingPlannerParameters p{}; + { + std::string ns{"constraints.hard"}; + p.max_curvature = node->declare_parameter(ns + ".max_curvature"); + p.min_curvature = node->declare_parameter(ns + ".min_curvature"); + ns = std::string{"constraints.soft"}; + p.lateral_deviation_weight = + node->declare_parameter(ns + ".lateral_deviation_weight"); // [[unused]] Delete? + p.length_weight = node->declare_parameter(ns + ".length_weight"); // [[unused]] Delete? + p.curvature_weight = + node->declare_parameter(ns + ".curvature_weight"); // [[unused]] Delete? + p.weights = node->declare_parameter>(ns + ".weights"); + } + { + std::string ns{"sampling"}; + p.enable_frenet = node->declare_parameter(ns + ".enable_frenet"); + p.enable_bezier = node->declare_parameter( + ns + ".enable_bezier"); // [[unused]] will be used in the future + p.resolution = node->declare_parameter(ns + ".resolution"); + p.previous_path_reuse_points_nb = + node->declare_parameter(ns + ".previous_path_reuse_points_nb"); + p.nb_target_lateral_positions = + node->declare_parameter(ns + ".nb_target_lateral_positions"); + p.target_lengths = node->declare_parameter>(ns + ".target_lengths"); + p.target_lateral_positions = + node->declare_parameter>(ns + ".target_lateral_positions"); + ns += ".frenet"; + p.target_lateral_velocities = + node->declare_parameter>(ns + ".target_lateral_velocities"); + p.target_lateral_accelerations = + node->declare_parameter>(ns + ".target_lateral_accelerations"); + } + { + std::string ns{"preprocessing"}; + p.force_zero_deviation = node->declare_parameter( + ns + ".force_zero_initial_deviation"); // [[unused]] will be used in the future + p.force_zero_heading = node->declare_parameter( + ns + ".force_zero_initial_heading"); // [[unused]] will be used in the future + p.smooth_reference = node->declare_parameter( + ns + ".smooth_reference_trajectory"); // [[unused]] will be used in the future + } + parameters_ = std::make_shared(p); +} + +void SamplingPlannerModuleManager::updateModuleParams( + [[maybe_unused]] const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + [[maybe_unused]] const std::string ns = name_ + "."; + + { + std::string ns{"constraints.hard"}; + updateParam(parameters, ns + ".max_curvature", p->max_curvature); + updateParam(parameters, ns + ".min_curvature", p->min_curvature); + ns = std::string{"constraints.soft"}; + updateParam(parameters, ns + ".lateral_deviation_weight", p->lateral_deviation_weight); + updateParam(parameters, ns + ".length_weight", p->length_weight); + updateParam(parameters, ns + ".curvature_weight", p->curvature_weight); + updateParam>(parameters, ns + ".weights", p->weights); + } + { + std::string ns{"sampling"}; + updateParam(parameters, ns + ".enable_frenet", p->enable_frenet); + updateParam(parameters, ns + ".enable_bezier", p->enable_bezier); + updateParam(parameters, ns + ".resolution", p->resolution); + + updateParam( + parameters, ns + ".previous_path_reuse_points_nb", p->previous_path_reuse_points_nb); + + updateParam( + parameters, ns + ".nb_target_lateral_positions", p->nb_target_lateral_positions); + updateParam>(parameters, ns + ".target_lengths", p->target_lengths); + + updateParam>( + parameters, ns + ".target_lateral_positions", p->target_lateral_positions); + + ns += ".frenet"; + updateParam>( + parameters, ns + ".target_lateral_velocities", p->target_lateral_velocities); + + updateParam>( + parameters, ns + ".target_lateral_accelerations", p->target_lateral_accelerations); + } + { + std::string ns{"preprocessing"}; + updateParam(parameters, ns + ".force_zero_initial_deviation", p->force_zero_deviation); + updateParam(parameters, ns + ".force_zero_initial_heading", p->force_zero_heading); + updateParam(parameters, ns + ".smooth_reference_trajectory", p->smooth_reference); + } + + std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) { + if (!observer.expired()) { + const auto sampling_planner_ptr = + std::dynamic_pointer_cast(observer.lock()); + if (sampling_planner_ptr) { + sampling_planner_ptr->updateModuleParams(p); + } + } + }); +} + +} // namespace autoware::behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_path_planner::SamplingPlannerModuleManager, + autoware::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp similarity index 90% rename from planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp rename to planning/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 4b0ecdcd24e14..9a7eb970e521a 100644 --- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_sampling_planner_module/sampling_planner_module.hpp" +#include "autoware_behavior_path_sampling_planner_module/sampling_planner_module.hpp" -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using geometry_msgs::msg::Point; using motion_utils::calcSignedArcLength; @@ -35,23 +35,24 @@ SamplingPlannerModule::SamplingPlannerModule( std::unordered_map> & objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()} { - internal_params_ = - std::shared_ptr(new SamplingPlannerInternalParameters{}); + internal_params_ = std::make_shared(); updateModuleParams(parameters); // check if the path is empty hard_constraints_.emplace_back( []( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const MultiPoint2d & footprint) -> bool { return !path.points.empty() && !path.poses.empty(); }); hard_constraints_.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint) -> bool { if (!footprint.empty()) { path.constraint_results.inside_drivable_area = @@ -71,7 +72,8 @@ SamplingPlannerModule::SamplingPlannerModule( hard_constraints_.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const MultiPoint2d & footprint) -> bool { if (path.curvatures.empty()) { path.constraint_results.valid_curvature = false; @@ -96,8 +98,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Yaw difference soft constraint cost -> Considering implementation // soft_constraints_.emplace_back( // [&]( - // sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & - // constraints, + // autoware::sampler_common::Path & path, [[maybe_unused]] const + // autoware::sampler_common::Constraints & constraints, // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { // if (path.points.empty()) return 0.0; // const auto & goal_pose_yaw = @@ -109,7 +111,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Remaining path length soft_constraints_.emplace_back( [&]( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.points.empty()) return 0.0; if (path.poses.empty()) return 0.0; @@ -142,8 +145,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Distance to centerline soft_constraints_.emplace_back( [&]( - [[maybe_unused]] sampler_common::Path & path, - [[maybe_unused]] const sampler_common::Constraints & constraints, + [[maybe_unused]] autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.poses.empty()) return 0.0; const auto & last_pose = path.poses.back(); @@ -159,7 +162,8 @@ SamplingPlannerModule::SamplingPlannerModule( // // Curvature cost soft_constraints_.emplace_back( []( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.curvatures.empty()) return std::numeric_limits::max(); @@ -244,7 +248,7 @@ bool SamplingPlannerModule::isReferencePathSafe() const std::vector hard_constraints_results; auto transform_to_sampling_path = [](const PlanResult plan) { - sampler_common::Path path; + autoware::sampler_common::Path path; for (size_t i = 0; i < plan->points.size(); ++i) { const auto x = plan->points[i].point.pose.position.x; const auto y = plan->points[i].point.pose.position.y; @@ -258,17 +262,20 @@ bool SamplingPlannerModule::isReferencePathSafe() const } return path; }; - sampler_common::Path reference_path = transform_to_sampling_path(prev_module_reference_path); - const auto footprint = sampler_common::constraints::buildFootprintPoints( + autoware::sampler_common::Path reference_path = + transform_to_sampling_path(prev_module_reference_path); + const auto footprint = autoware::sampler_common::constraints::buildFootprintPoints( reference_path, internal_params_->constraints); - behavior_path_planner::HardConstraintsFunctionVector hard_constraints_reference_path; + HardConstraintsFunctionVector hard_constraints_reference_path; hard_constraints_reference_path.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint) -> bool { path.constraint_results.collision_free = - !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons); + !autoware::sampler_common::constraints::has_collision( + footprint, constraints.obstacle_polygons); return path.constraint_results.collision_free; }); evaluateHardConstraints( @@ -295,7 +302,7 @@ SamplingPlannerData SamplingPlannerModule::createPlannerData( } PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( - const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, + const autoware::frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, const double path_z) { auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion { @@ -348,12 +355,12 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( } void SamplingPlannerModule::prepareConstraints( - sampler_common::Constraints & constraints, + autoware::sampler_common::Constraints & constraints, const PredictedObjects::ConstSharedPtr & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) const { - constraints.obstacle_polygons = sampler_common::MultiPolygon2d(); + constraints.obstacle_polygons = autoware::sampler_common::MultiPolygon2d(); constraints.rtree.clear(); size_t i = 0; for (const auto & o : predicted_objects->objects) { @@ -370,7 +377,7 @@ void SamplingPlannerModule::prepareConstraints( // TODO(Maxime): directly use lines instead of polygon - sampler_common::Polygon2d drivable_area_polygon; + autoware::sampler_common::Polygon2d drivable_area_polygon; for (const auto & p : right_bound) { drivable_area_polygon.outer().emplace_back(p.x, p.y); } @@ -392,7 +399,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() if (reference_path_ptr->points.empty()) { return {}; } - auto reference_spline = [&]() -> sampler_common::transform::Spline2D { + auto reference_spline = [&]() -> autoware::sampler_common::transform::Spline2D { std::vector x; std::vector y; x.reserve(reference_path_ptr->points.size()); @@ -404,19 +411,18 @@ BehaviorModuleOutput SamplingPlannerModule::plan() return {x, y}; }(); - frenet_planner::FrenetState frenet_initial_state; - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::FrenetState frenet_initial_state; + autoware::frenet_planner::SamplingParameters sampling_parameters; const auto & pose = planner_data_->self_odometry->pose.pose; - sampler_common::State initial_state = - behavior_path_planner::getInitialState(pose, reference_spline); + autoware::sampler_common::State initial_state = getInitialState(pose, reference_spline); sampling_parameters = prepareSamplingParameters(initial_state, reference_spline, *internal_params_); auto set_frenet_state = []( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & reference_spline, - frenet_planner::FrenetState & frenet_initial_state) + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & reference_spline, + autoware::frenet_planner::FrenetState & frenet_initial_state) { frenet_initial_state.position = initial_state.frenet; @@ -496,14 +502,16 @@ BehaviorModuleOutput SamplingPlannerModule::plan() const int path_divisions = internal_params_->sampling.previous_path_reuse_points_nb; const bool is_extend_previous_path = path_divisions > 0; - std::vector frenet_paths; + std::vector frenet_paths; // Extend prev path if (prev_path_is_valid && is_extend_previous_path) { - frenet_planner::Path prev_path_frenet = prev_sampling_path_.value(); + autoware::frenet_planner::Path prev_path_frenet = prev_sampling_path_.value(); frenet_paths.push_back(prev_path_frenet); - auto get_subset = [](const frenet_planner::Path & path, size_t offset) -> frenet_planner::Path { - frenet_planner::Path s; + auto get_subset = []( + const autoware::frenet_planner::Path & path, + size_t offset) -> autoware::frenet_planner::Path { + autoware::frenet_planner::Path s; s.points = {path.points.begin(), path.points.begin() + offset}; s.curvatures = {path.curvatures.begin(), path.curvatures.begin() + offset}; s.yaws = {path.yaws.begin(), path.yaws.begin() + offset}; @@ -512,7 +520,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() return s; }; - sampler_common::State current_state; + autoware::sampler_common::State current_state; current_state.pose = {ego_pose.position.x, ego_pose.position.y}; const auto closest_iter = std::min_element( @@ -537,22 +545,21 @@ BehaviorModuleOutput SamplingPlannerModule::plan() const auto reused_path = get_subset(prev_path_frenet, reuse_idx); geometry_msgs::msg::Pose future_pose = reused_path.poses.back(); - sampler_common::State future_state = - behavior_path_planner::getInitialState(future_pose, reference_spline); - frenet_planner::FrenetState frenet_reuse_state; + autoware::sampler_common::State future_state = getInitialState(future_pose, reference_spline); + autoware::frenet_planner::FrenetState frenet_reuse_state; set_frenet_state(future_state, reference_spline, frenet_reuse_state); - frenet_planner::SamplingParameters extension_sampling_parameters = + autoware::frenet_planner::SamplingParameters extension_sampling_parameters = prepareSamplingParameters(future_state, reference_spline, *internal_params_); - auto extension_frenet_paths = frenet_planner::generatePaths( + auto extension_frenet_paths = autoware::frenet_planner::generatePaths( reference_spline, frenet_reuse_state, extension_sampling_parameters); for (auto & p : extension_frenet_paths) { if (!p.points.empty()) frenet_paths.push_back(reused_path.extend(p)); } } } else { - frenet_paths = - frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters); + frenet_paths = autoware::frenet_planner::generatePaths( + reference_spline, frenet_initial_state, sampling_parameters); } const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path; @@ -587,8 +594,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan() std::vector> hard_constraints_results_full; std::vector> soft_constraints_results_full; for (auto & path : frenet_paths) { - const auto footprint = - sampler_common::constraints::buildFootprintPoints(path, internal_params_->constraints); + const auto footprint = autoware::sampler_common::constraints::buildFootprintPoints( + path, internal_params_->constraints); std::vector hard_constraints_results = evaluateHardConstraints(path, internal_params_->constraints, footprint, hard_constraints_); path.constraint_results.valid_curvature = true; @@ -598,7 +605,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() soft_constraints_results_full.push_back(soft_constraints_results); } - std::vector candidate_paths; + std::vector candidate_paths; const auto move_to_paths = [&candidate_paths](auto & paths_to_move) { candidate_paths.insert( candidate_paths.end(), std::make_move_iterator(paths_to_move.begin()), @@ -917,9 +924,9 @@ DrivableLanes SamplingPlannerModule::generateExpandDrivableLanes( return current_drivable_lanes; } -frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, +autoware::frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const SamplingPlannerInternalParameters & params_) { // calculate target lateral positions @@ -953,10 +960,10 @@ frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParamet } else { target_lateral_positions = params_.sampling.target_lateral_positions; } - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::SamplingParameters sampling_parameters; sampling_parameters.resolution = params_.sampling.resolution; const auto max_s = path_spline.lastS(); - frenet_planner::SamplingParameter p; + autoware::frenet_planner::SamplingParameter p; for (const auto target_length : params_.sampling.target_lengths) { p.target_state.position.s = std::min(max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length)); @@ -975,4 +982,4 @@ frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParamet return sampling_parameters; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_side_shift_module/CMakeLists.txt b/planning/autoware_behavior_path_side_shift_module/CMakeLists.txt new file mode 100644 index 0000000000000..520132dbd3363 --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_path_side_shift_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene.cpp + src/utils.cpp + src/manager.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME} + test/test_behavior_path_planner_node_interface.cpp + ) + + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ) + + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_side_shift_module/README.md b/planning/autoware_behavior_path_side_shift_module/README.md similarity index 100% rename from planning/behavior_path_side_shift_module/README.md rename to planning/autoware_behavior_path_side_shift_module/README.md diff --git a/planning/behavior_path_side_shift_module/config/side_shift.param.yaml b/planning/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml similarity index 100% rename from planning/behavior_path_side_shift_module/config/side_shift.param.yaml rename to planning/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml diff --git a/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg b/planning/autoware_behavior_path_side_shift_module/images/side_shift_status.drawio.svg similarity index 100% rename from planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg rename to planning/autoware_behavior_path_side_shift_module/images/side_shift_status.drawio.svg diff --git a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp new file mode 100644 index 0000000000000..9140132de1335 --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp @@ -0,0 +1,54 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ + +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using tier4_planning_msgs::msg::LateralOffset; + +enum class SideShiftStatus { STOP = 0, BEFORE_SHIFT, SHIFTING, AFTER_SHIFT }; + +struct SideShiftParameters +{ + double time_to_start_shifting; + double min_distance_to_start_shifting; + double shifting_lateral_jerk; + double min_shifting_distance; + double min_shifting_speed; + double drivable_area_resolution; + double drivable_area_width; + double drivable_area_height; + double shift_request_time_limit; + bool publish_debug_marker; +}; + +struct SideShiftDebugData +{ + std::shared_ptr path_shifter{}; + ShiftLineArray shift_lines{}; + double current_request{0.0}; +}; + +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp new file mode 100644 index 0000000000000..339d1e2679e31 --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.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 AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ + +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_behavior_path_side_shift_module/scene.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ + +class SideShiftModuleManager : public SceneModuleManagerInterface +{ +public: + SideShiftModuleManager() : SceneModuleManagerInterface{"side_shift"} {} + + void init(rclcpp::Node * node) override; + + std::unique_ptr createNewSceneModuleInstance() override + { + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); + } + + void updateModuleParams(const std::vector & parameters) override; + +private: + std::shared_ptr parameters_; +}; + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp new file mode 100644 index 0000000000000..d2ef96056a9ce --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp @@ -0,0 +1,134 @@ +// Copyright 2021 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ + +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_side_shift_module/data_structs.hpp" + +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using geometry_msgs::msg::Pose; +using nav_msgs::msg::OccupancyGrid; +using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; + +class SideShiftModule : public SceneModuleInterface +{ +public: + SideShiftModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); + + bool isExecutionRequested() const override; + bool isExecutionReady() const override; + bool isReadyForNextRequest( + const double & min_request_time_sec, bool override_requests = false) const noexcept; + void updateData() override; + BehaviorModuleOutput plan() override; + BehaviorModuleOutput planWaitingApproval() override; + CandidateOutput planCandidate() const override; + void processOnEntry() override; + void processOnExit() override; + + void setParameters(const std::shared_ptr & parameters); + + void updateModuleParams(const std::any & parameters) override + { + parameters_ = std::any_cast>(parameters); + } + + void acceptVisitor( + [[maybe_unused]] const std::shared_ptr & visitor) const override + { + } + +private: + bool canTransitSuccessState() override; + + bool canTransitFailureState() override { return false; } + + void initVariables(); + + // non-const methods + BehaviorModuleOutput adjustDrivableArea(const ShiftedPath & path) const; + + ShiftLine calcShiftLine() const; + + void replaceShiftLine(); + + // const methods + void publishPath(const PathWithLaneId & path) const; + + double getClosestShiftLength() const; + + // member + PathWithLaneId refined_path_{}; + PathWithLaneId reference_path_{}; + lanelet::ConstLanelets current_lanelets_; + std::shared_ptr parameters_; + + // Requested lateral offset to shift the reference path. + double requested_lateral_offset_{0.0}; + + // Inserted lateral offset to shift the reference path. + double inserted_lateral_offset_{0.0}; + + // Inserted shift lines in the path + ShiftLine inserted_shift_line_; + + // Shift status + SideShiftStatus shift_status_; + + // Flag to check lateral offset change is requested + bool lateral_offset_change_request_{false}; + + // Triggered when offset is changed, released when start pose is refound. + bool start_pose_reset_request_{false}; + + PathShifter path_shifter_; + + ShiftedPath prev_output_; + ShiftLine prev_shift_line_; + + PathWithLaneId extendBackwardLength(const PathWithLaneId & original_path) const; + + mutable rclcpp::Time last_requested_shift_change_time_{clock_->now()}; + + rclcpp::Time latest_lateral_offset_stamp_; + + // debug + mutable SideShiftDebugData debug_data_; + void setDebugMarkersVisualization() const; +}; + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ diff --git a/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp new file mode 100644 index 0000000000000..bda216b812467 --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp @@ -0,0 +1,44 @@ +// Copyright 2021 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::TransformStamped; +using tier4_planning_msgs::msg::PathWithLaneId; + +void setOrientation(PathWithLaneId * path); + +bool getStartAvoidPose( + const PathWithLaneId & path, const double start_distance, const size_t nearest_idx, + Pose * start_avoid_pose); + +bool isAlmostZero(double v); + +Point transformToGrid( + const Point & pt, const double longitudinal_offset, const double lateral_offset, const double yaw, + const TransformStamped & geom_tf); + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_side_shift_module/package.xml b/planning/autoware_behavior_path_side_shift_module/package.xml new file mode 100644 index 0000000000000..f3ac76d2bb1bb --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/package.xml @@ -0,0 +1,38 @@ + + + + autoware_behavior_path_side_shift_module + 0.1.0 + The autoware_behavior_path_side_shift_module package + + Satoshi Ota + Fumiya Watanabe + Kyoichi Sugahara + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_path_planner + autoware_behavior_path_planner_common + autoware_planning_msgs + geometry_msgs + motion_utils + pluginlib + rclcpp + tier4_autoware_utils + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_path_side_shift_module/plugins.xml b/planning/autoware_behavior_path_side_shift_module/plugins.xml new file mode 100644 index 0000000000000..c06d6405525dc --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/autoware_behavior_path_side_shift_module/src/manager.cpp b/planning/autoware_behavior_path_side_shift_module/src/manager.cpp new file mode 100644 index 0000000000000..e1ac25726d6d0 --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/src/manager.cpp @@ -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. + +#include "autoware_behavior_path_side_shift_module/manager.hpp" + +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ + +void SideShiftModuleManager::init(rclcpp::Node * node) +{ + // init manager interface + initInterface(node, {}); + + SideShiftParameters p{}; + + const std::string ns = "side_shift."; + p.min_distance_to_start_shifting = + node->declare_parameter(ns + "min_distance_to_start_shifting"); + p.time_to_start_shifting = node->declare_parameter(ns + "time_to_start_shifting"); + p.shifting_lateral_jerk = node->declare_parameter(ns + "shifting_lateral_jerk"); + p.min_shifting_distance = node->declare_parameter(ns + "min_shifting_distance"); + p.min_shifting_speed = node->declare_parameter(ns + "min_shifting_speed"); + p.shift_request_time_limit = node->declare_parameter(ns + "shift_request_time_limit"); + p.publish_debug_marker = node->declare_parameter(ns + "publish_debug_marker"); + + parameters_ = std::make_shared(p); +} + +void SideShiftModuleManager::updateModuleParams( + [[maybe_unused]] const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + [[maybe_unused]] auto p = parameters_; + + [[maybe_unused]] const std::string ns = "side_shift."; + // updateParam(parameters, ns + ..., ...); + + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); + }); +} + +} // namespace autoware::behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_path_planner::SideShiftModuleManager, + autoware::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/autoware_behavior_path_side_shift_module/src/scene.cpp new file mode 100644 index 0000000000000..2ddd0de4cddfc --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -0,0 +1,484 @@ +// Copyright 2021 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 "autoware_behavior_path_side_shift_module/scene.hpp" + +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_side_shift_module/utils.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using geometry_msgs::msg::Point; +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; +using motion_utils::findNearestSegmentIndex; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::getPoint; + +SideShiftModule::SideShiftModule( + const std::string & name, rclcpp::Node & node, + const std::shared_ptr & parameters, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + parameters_{parameters} +{ +} + +void SideShiftModule::initVariables() +{ + reference_path_ = PathWithLaneId(); + debug_data_.path_shifter.reset(); + debug_marker_.markers.clear(); + start_pose_reset_request_ = false; + requested_lateral_offset_ = 0.0; + inserted_lateral_offset_ = 0.0; + inserted_shift_line_ = ShiftLine{}; + shift_status_ = SideShiftStatus{}; + prev_output_ = ShiftedPath{}; + prev_shift_line_ = ShiftLine{}; + path_shifter_ = PathShifter{}; + resetPathCandidate(); + resetPathReference(); +} + +void SideShiftModule::processOnEntry() +{ + // write me... (Don't initialize variables, otherwise lateral offset gets zero on entry.) + start_pose_reset_request_ = false; +} + +void SideShiftModule::processOnExit() +{ + // write me... + initVariables(); +} + +void SideShiftModule::setParameters(const std::shared_ptr & parameters) +{ + parameters_ = parameters; +} + +bool SideShiftModule::isExecutionRequested() const +{ + if (getCurrentStatus() == ModuleStatus::RUNNING) { + return true; + } + + // If the desired offset has a non-zero value, return true as we want to execute the plan. + + const bool has_request = !isAlmostZero(requested_lateral_offset_); + RCLCPP_DEBUG_STREAM( + getLogger(), "ESS::isExecutionRequested() : " << std::boolalpha << has_request); + + return has_request; +} + +bool SideShiftModule::isExecutionReady() const +{ + return true; // TODO(Horibe) is it ok to say "always safe"? +} + +bool SideShiftModule::isReadyForNextRequest( + const double & min_request_time_sec, bool override_requests) const noexcept +{ + rclcpp::Time current_time = clock_->now(); + const auto interval_from_last_request_sec = current_time - last_requested_shift_change_time_; + + if (interval_from_last_request_sec.seconds() >= min_request_time_sec && !override_requests) { + last_requested_shift_change_time_ = current_time; + return true; + } + + return false; +} + +bool SideShiftModule::canTransitSuccessState() +{ + // Never return the FAILURE. When the desired offset is zero and the vehicle is in the original + // drivable area,this module can stop the computation and return SUCCESS. + + const auto isOffsetDiffAlmostZero = [this]() noexcept { + const auto last_sp = path_shifter_.getLastShiftLine(); + if (last_sp) { + const auto length = std::fabs(last_sp.value().end_shift_length); + const auto lateral_offset = std::fabs(requested_lateral_offset_); + const auto offset_diff = lateral_offset - length; + if (!isAlmostZero(offset_diff)) { + lateral_offset_change_request_ = true; + return false; + } + } + return true; + }(); + + const bool no_offset_diff = isOffsetDiffAlmostZero; + const bool no_request = isAlmostZero(requested_lateral_offset_); + + const auto no_shifted_plan = [&]() { + if (prev_output_.shift_length.empty()) { + return true; + } else { + const auto max_planned_shift_length = std::max_element( + prev_output_.shift_length.begin(), prev_output_.shift_length.end(), + [](double a, double b) { return std::abs(a) < std::abs(b); }); + return std::abs(*max_planned_shift_length) < 0.01; + } + }(); + + RCLCPP_DEBUG( + getLogger(), "ESS::updateState() : no_request = %d, no_shifted_plan = %d", no_request, + no_shifted_plan); + + if (no_request && no_shifted_plan && no_offset_diff) { + return true; + } + + const auto & current_lanes = utils::getCurrentLanes(planner_data_); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & inserted_shift_line_start_pose = inserted_shift_line_.start; + const auto & inserted_shift_line_end_pose = inserted_shift_line_.end; + const double self_to_shift_line_start_arc_length = + autoware::behavior_path_planner::utils::getSignedDistance( + current_pose, inserted_shift_line_start_pose, current_lanes); + const double self_to_shift_line_end_arc_length = + autoware::behavior_path_planner::utils::getSignedDistance( + current_pose, inserted_shift_line_end_pose, current_lanes); + if (self_to_shift_line_start_arc_length >= 0) { + shift_status_ = SideShiftStatus::BEFORE_SHIFT; + } else if (self_to_shift_line_start_arc_length < 0 && self_to_shift_line_end_arc_length > 0) { + shift_status_ = SideShiftStatus::SHIFTING; + } else { + shift_status_ = SideShiftStatus::AFTER_SHIFT; + } + return false; +} + +void SideShiftModule::updateData() +{ + if ( + planner_data_->lateral_offset != nullptr && + planner_data_->lateral_offset->stamp != latest_lateral_offset_stamp_) { + if (isReadyForNextRequest(parameters_->shift_request_time_limit)) { + lateral_offset_change_request_ = true; + requested_lateral_offset_ = planner_data_->lateral_offset->lateral_offset; + latest_lateral_offset_stamp_ = planner_data_->lateral_offset->stamp; + } + } + + if (getCurrentStatus() != ModuleStatus::RUNNING && getCurrentStatus() != ModuleStatus::IDLE) { + return; + } + + // special for avoidance: take behind distance upt ot shift-start-point if it exist. + const auto longest_dist_to_shift_line = [&]() { + double max_dist = 0.0; + for (const auto & pnt : path_shifter_.getShiftLines()) { + max_dist = std::max(max_dist, tier4_autoware_utils::calcDistance2d(getEgoPose(), pnt.start)); + } + return max_dist; + }(); + + const auto reference_pose = prev_output_.shift_length.empty() + ? planner_data_->self_odometry->pose.pose + : utils::getUnshiftedEgoPose(getEgoPose(), prev_output_); + if (getPreviousModuleOutput().reference_path.points.empty()) { + return; + } + const auto centerline_path = utils::calcCenterLinePath( + planner_data_, reference_pose, longest_dist_to_shift_line, + getPreviousModuleOutput().reference_path); + + constexpr double resample_interval = 1.0; + const auto backward_extened_path = extendBackwardLength(getPreviousModuleOutput().path); + reference_path_ = utils::resamplePathWithSpline(backward_extened_path, resample_interval); + + path_shifter_.setPath(reference_path_); + + const auto & route_handler = planner_data_->route_handler; + const auto & p = planner_data_->parameters; + + lanelet::ConstLanelet current_lane; + if (!route_handler->getClosestLaneletWithinRoute(reference_pose, ¤t_lane)) { + RCLCPP_ERROR_THROTTLE( + getLogger(), *clock_, 5000, "failed to find closest lanelet within route!!!"); + } + + // For current_lanes with desired length + current_lanelets_ = route_handler->getLaneletSequence( + current_lane, reference_pose, p.backward_path_length, p.forward_path_length); + + const size_t nearest_idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); + path_shifter_.removeBehindShiftLineAndSetBaseOffset(nearest_idx); +} + +void SideShiftModule::replaceShiftLine() +{ + auto shift_lines = path_shifter_.getShiftLines(); + if (shift_lines.size() > 0) { + shift_lines.clear(); + } + + const auto new_sl = calcShiftLine(); + + // if no conflict, then add the new point. + shift_lines.push_back(new_sl); + const bool new_sl_is_same_with_previous = + new_sl.end_shift_length == prev_shift_line_.end_shift_length; + + if (!new_sl_is_same_with_previous) { + prev_shift_line_ = new_sl; + } + + // set to path_shifter + path_shifter_.setShiftLines(shift_lines); + lateral_offset_change_request_ = false; + inserted_lateral_offset_ = requested_lateral_offset_; + inserted_shift_line_ = new_sl; + + return; +} + +BehaviorModuleOutput SideShiftModule::plan() +{ + // Replace shift line + if ( + lateral_offset_change_request_ && ((shift_status_ == SideShiftStatus::BEFORE_SHIFT) || + (shift_status_ == SideShiftStatus::AFTER_SHIFT))) { + replaceShiftLine(); + } else if (shift_status_ != SideShiftStatus::BEFORE_SHIFT) { + RCLCPP_DEBUG(getLogger(), "ego is shifting"); + } else { + RCLCPP_DEBUG(getLogger(), "change is not requested"); + } + + // Refine path + ShiftedPath shifted_path; + path_shifter_.generate(&shifted_path); + + // Reset orientation + setOrientation(&shifted_path.path); + + auto output = adjustDrivableArea(shifted_path); + output.reference_path = getPreviousModuleOutput().reference_path; + + prev_output_ = shifted_path; + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); + + debug_data_.path_shifter = std::make_shared(path_shifter_); + + if (parameters_->publish_debug_marker) { + setDebugMarkersVisualization(); + } else { + debug_marker_.markers.clear(); + } + + return output; +} + +CandidateOutput SideShiftModule::planCandidate() const +{ + auto path_shifter_local = path_shifter_; + + path_shifter_local.addShiftLine(calcShiftLine()); + + // Refine path + ShiftedPath shifted_path; + path_shifter_local.generate(&shifted_path); + + // Reset orientation + setOrientation(&shifted_path.path); + + return CandidateOutput(shifted_path.path); +} + +BehaviorModuleOutput SideShiftModule::planWaitingApproval() +{ + // Refine path + ShiftedPath shifted_path; + path_shifter_.generate(&shifted_path); + + // Reset orientation + setOrientation(&shifted_path.path); + + auto output = adjustDrivableArea(shifted_path); + + output.reference_path = getPreviousModuleOutput().reference_path; + + path_candidate_ = std::make_shared(planCandidate().path_candidate); + path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); + + prev_output_ = shifted_path; + + return output; +} + +ShiftLine SideShiftModule::calcShiftLine() const +{ + const auto & p = parameters_; + const auto ego_speed = std::abs(planner_data_->self_odometry->twist.twist.linear.x); + + const double dist_to_start = + std::max(p->min_distance_to_start_shifting, ego_speed * p->time_to_start_shifting); + + const double dist_to_end = [&]() { + const double shift_length = requested_lateral_offset_ - getClosestShiftLength(); + const double jerk_shifting_distance = path_shifter_.calcLongitudinalDistFromJerk( + shift_length, p->shifting_lateral_jerk, std::max(ego_speed, p->min_shifting_speed)); + const double shifting_distance = std::max(jerk_shifting_distance, p->min_shifting_distance); + const double dist_to_end = dist_to_start + shifting_distance; + RCLCPP_DEBUG( + getLogger(), "min_distance_to_start_shifting = %f, dist_to_start = %f, dist_to_end = %f", + parameters_->min_distance_to_start_shifting, dist_to_start, dist_to_end); + return dist_to_end; + }(); + + const size_t nearest_idx = planner_data_->findEgoIndex(reference_path_.points); + ShiftLine shift_line; + shift_line.end_shift_length = requested_lateral_offset_; + shift_line.start_idx = utils::getIdxByArclength(reference_path_, nearest_idx, dist_to_start); + shift_line.start = reference_path_.points.at(shift_line.start_idx).point.pose; + shift_line.end_idx = utils::getIdxByArclength(reference_path_, nearest_idx, dist_to_end); + shift_line.end = reference_path_.points.at(shift_line.end_idx).point.pose; + + return shift_line; +} + +double SideShiftModule::getClosestShiftLength() const +{ + if (prev_output_.shift_length.empty()) { + return 0.0; + } + + const auto ego_point = planner_data_->self_odometry->pose.pose.position; + const auto closest = motion_utils::findNearestIndex(prev_output_.path.points, ego_point); + return prev_output_.shift_length.at(closest); +} + +BehaviorModuleOutput SideShiftModule::adjustDrivableArea(const ShiftedPath & path) const +{ + BehaviorModuleOutput out; + const auto & p = planner_data_->parameters; + + const auto & dp = planner_data_->drivable_area_expansion_parameters; + const auto itr = std::minmax_element(path.shift_length.begin(), path.shift_length.end()); + + constexpr double threshold = 0.1; + constexpr double margin = 0.5; + const double left_offset = std::max( + *itr.second + (*itr.first > threshold ? margin : 0.0), dp.drivable_area_left_bound_offset); + const double right_offset = -std::min( + *itr.first - (*itr.first < -threshold ? margin : 0.0), -dp.drivable_area_right_bound_offset); + + // crop path which is too long here + auto output_path = path.path; + const size_t current_seg_idx = planner_data_->findEgoSegmentIndex(output_path.points); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + output_path.points = motion_utils::cropPoints( + output_path.points, current_pose.position, current_seg_idx, p.forward_path_length, + p.backward_path_length + p.input_path_interval); + + const auto drivable_lanes = utils::generateDrivableLanes(current_lanelets_); + const auto shorten_lanes = utils::cutOverlappedLanes(output_path, drivable_lanes); + const auto expanded_lanes = + utils::expandLanelets(shorten_lanes, left_offset, right_offset, dp.drivable_area_types_to_skip); + + { // for new architecture + // NOTE: side shift module is not launched with other modules. Therefore, drivable_lanes can be + // assigned without combining. + out.path = output_path; + out.reference_path = getPreviousModuleOutput().reference_path; + out.drivable_area_info.drivable_lanes = expanded_lanes; + out.drivable_area_info.is_already_expanded = true; + } + + return out; +} + +PathWithLaneId SideShiftModule::extendBackwardLength(const PathWithLaneId & original_path) const +{ + // special for avoidance: take behind distance upt ot shift-start-point if it exist. + 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)); + } + return max_dist; + }(); + + 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 & prev_reference = getPreviousModuleOutput().path; + const size_t orig_ego_idx = findNearestIndex(original_path.points, getEgoPose().position); + const size_t prev_ego_idx = + findNearestSegmentIndex(prev_reference.points, getPoint(original_path.points.at(orig_ego_idx))); + + size_t clip_idx = 0; + for (size_t i = 0; i < prev_ego_idx; ++i) { + if (backward_length > calcSignedArcLength(prev_reference.points, clip_idx, prev_ego_idx)) { + break; + } + clip_idx = i; + } + + PathWithLaneId extended_path{}; + { + extended_path.points.insert( + extended_path.points.end(), prev_reference.points.begin() + clip_idx, + prev_reference.points.begin() + prev_ego_idx); + } + + { + extended_path.points.insert( + extended_path.points.end(), original_path.points.begin() + orig_ego_idx, + original_path.points.end()); + } + + return extended_path; +} + +void SideShiftModule::setDebugMarkersVisualization() const +{ + using marker_utils::createShiftLineMarkerArray; + + debug_marker_.markers.clear(); + + const auto add = [this](const MarkerArray & added) { + tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); + }; + + const auto add_shift_line_marker = [this, add]( + const auto & ns, auto r, auto g, auto b, double w = 0.1) { + add(createShiftLineMarkerArray( + debug_data_.path_shifter->getShiftLines(), debug_data_.path_shifter->getBaseOffset(), ns, r, + g, b, w)); + }; + + if (debug_data_.path_shifter) { + add_shift_line_marker("side_shift_shift_points", 0.7, 0.7, 0.7, 0.4); + } +} +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_side_shift_module/src/utils.cpp b/planning/autoware_behavior_path_side_shift_module/src/utils.cpp new file mode 100644 index 0000000000000..783874720b17d --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/src/utils.cpp @@ -0,0 +1,101 @@ +// Copyright 2021 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 "autoware_behavior_path_side_shift_module/utils.hpp" + +#include "autoware_behavior_path_planner_common/utils/utils.hpp" + +#include + +#include + +#include + +namespace autoware::behavior_path_planner +{ +void setOrientation(PathWithLaneId * path) +{ + if (!path) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("side_shift").get_child("util"), + "Pointer to path is NULL!"); + } + + // Reset orientation + for (size_t idx = 0; idx < path->points.size(); ++idx) { + double angle = 0.0; + auto & pt = path->points.at(idx); + if (idx + 1 < path->points.size()) { + const auto next_pt = path->points.at(idx + 1); + angle = std::atan2( + next_pt.point.pose.position.y - pt.point.pose.position.y, + next_pt.point.pose.position.x - pt.point.pose.position.x); + } else if (idx != 0) { + const auto prev_pt = path->points.at(idx - 1); + angle = std::atan2( + pt.point.pose.position.y - prev_pt.point.pose.position.y, + pt.point.pose.position.x - prev_pt.point.pose.position.x); + } + tf2::Quaternion yaw_quat; + yaw_quat.setRPY(0, 0, angle); + pt.point.pose.orientation = tf2::toMsg(yaw_quat); + } +} + +bool getStartAvoidPose( + const PathWithLaneId & path, const double start_distance, const size_t nearest_idx, + Pose * start_avoid_pose) +{ + if (!start_avoid_pose) { + return false; + } + if (nearest_idx >= path.points.size()) { + return false; + } + + double arclength = 0.0; + for (size_t idx = nearest_idx + 1; idx < path.points.size(); ++idx) { + const auto pt = path.points.at(idx).point; + const auto pt_prev = path.points.at(idx - 1).point; + const double dx = pt.pose.position.x - pt_prev.pose.position.x; + const double dy = pt.pose.position.y - pt_prev.pose.position.y; + arclength += std::hypot(dx, dy); + + if (arclength > start_distance) { + *start_avoid_pose = pt.pose; + return true; + } + } + + return false; +} + +bool isAlmostZero(double v) +{ + return std::fabs(v) < 1.0e-4; +} + +Point transformToGrid( + const Point & pt, const double longitudinal_offset, const double lateral_offset, const double yaw, + const TransformStamped & geom_tf) +{ + Point offset_pt, grid_pt; + offset_pt = pt; + offset_pt.x += longitudinal_offset * cos(yaw) - lateral_offset * sin(yaw); + offset_pt.y += longitudinal_offset * sin(yaw) + lateral_offset * cos(yaw); + tf2::doTransform(offset_pt, grid_pt, geom_tf); + return grid_pt; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp new file mode 100644 index 0000000000000..266054e9d46f2 --- /dev/null +++ b/planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -0,0 +1,126 @@ +// 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 "autoware_behavior_path_planner/behavior_path_planner_node.hpp" + +#include +#include +#include + +#include + +#include +#include + +using autoware::behavior_path_planner::BehaviorPathPlannerNode; +using planning_test_utils::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_path_planner → test_node_ + test_manager->setPathWithLaneIdSubscriber("behavior_path_planner/output/path"); + + // set behavior_path_planner's input topic name(this topic is changed to test node) + test_manager->setRouteInputTopicName("behavior_path_planner/input/route"); + + test_manager->setInitialPoseTopicName("behavior_path_planner/input/odometry"); + + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + const auto planning_test_utils_dir = + ament_index_cpp::get_package_share_directory("planning_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("behavior_path_planner"); + + std::vector module_names; + module_names.emplace_back("autoware::behavior_path_planner::SideShiftModuleManager"); + + std::vector params; + params.emplace_back("launch_modules", module_names); + node_options.parameter_overrides(params); + + test_utils::updateNodeOptions( + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_side_shift_module") + + "/config/side_shift.param.yaml"}); + + return std::make_shared(node_options); +} + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + // publish necessary topics from test_manager + test_manager->publishInitialPose(test_target_node, "behavior_path_planner/input/odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_path_planner/input/accel"); + test_manager->publishPredictedObjects(test_target_node, "behavior_path_planner/input/perception"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_path_planner/input/occupancy_grid_map"); + test_manager->publishLaneDrivingScenario( + test_target_node, "behavior_path_planner/input/scenario"); + test_manager->publishMap(test_target_node, "behavior_path_planner/input/vector_map"); + test_manager->publishCostMap(test_target_node, "behavior_path_planner/input/costmap"); + test_manager->publishOperationModeState(test_target_node, "system/operation_mode/state"); + test_manager->publishLateralOffset( + test_target_node, "behavior_path_planner/input/lateral_offset"); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) +{ + rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty route + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalRoute(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithBehaviorNominalRoute(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testRouteWithInvalidEgoPose(test_target_node)); + + rclcpp::shutdown(); +} diff --git a/planning/autoware_behavior_path_start_planner_module/CMakeLists.txt b/planning/autoware_behavior_path_start_planner_module/CMakeLists.txt new file mode 100644 index 0000000000000..1d0aa23200f96 --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_path_start_planner_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/start_planner_module.cpp + src/manager.cpp + src/freespace_pull_out.cpp + src/geometric_pull_out.cpp + src/shift_pull_out.cpp + src/util.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/autoware_behavior_path_start_planner_module/README.md b/planning/autoware_behavior_path_start_planner_module/README.md new file mode 100644 index 0000000000000..b2c92f31ded31 --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/README.md @@ -0,0 +1,548 @@ +# Start Planner design + +## Purpose / Role + +This module generates and plans a path for safely merging from the shoulder lane or side of road lane into the center of the road lane. + +Specifically, it includes the following features: + +- Plan the path to automatically start from the shoulder lane or side of road lane to center of road lane. +- When parked vehicles are present on the shoulder lane, the module generates a path that allows for starting with a gap of a specified margin. +- If a collision with other traffic participants is detected while traveling on the generated path, it will stop as much as possible. + +
+ ![start_planner_module](images/start_planner_example.png){width=1100} +
+ +## Use Cases + +Give an typical example of how path generation is executed. Showing example of path generation starts from shoulder lane, but also from side of road lane can be generated. + +
+ ![shift_pull_out_from_road_lane](images/shift_pull_out_path_from_road_lane.drawio.svg){width=1100} +
+ +### **Use Case 1: Shift pull out** + +In the shoulder lane, when there are no parked vehicles ahead and the shoulder lane is sufficiently long, a forward shift pull out path (a clothoid curve with consistent jerk) is generated. + +
+ ![shift_pull_out](images/shift_pull_out_path.drawio.svg){width=1100} +
+ +### **Use Case 2: Geometric pull out** + +In the shoulder lane, when there are objects in front and the lane is not sufficiently long behind, a geometric pull out path is generated. + +
+ ![geometric_pull_out](images/geometric_pull_out_path.drawio.svg){width=1100} +
+ +### **Use Case 3: Backward and shift pull out** + +In the shoulder lane, when there are parked vehicles ahead and the lane is sufficiently long behind, a path that involves reversing before generating a forward shift pull out path is created. + +
+ ![shift_pull_out_with_back](images/shift_pull_out_path_with_back.drawio.svg){width=1100} +
+ +### **Use Case 4: Backward and geometric pull out** + +In the shoulder lane, when there is an object ahead and not enough space to reverse sufficiently, a path that involves reversing before making an geometric pull out is generated. + +
+ ![geometric_pull_out_with_back](images/geometric_pull_out_path_with_back.drawio.svg){width=1100} +
+ +### **Use Case 5: Freespace pull out** + +If the map is annotated with the information that a free space path can be generated in situations where both shift and geometric pull out paths are impossible to create, a path based on the free space algorithm will be generated. + +
+ ![freespace_path](images/freespace_pull_out.png){width=1100} +
+ +**As a note, the patterns for generating these paths are based on default parameters, but as will be explained in the following sections, it is possible to control aspects such as making paths that involve reversing more likely to be generated, or making geometric paths more likely to be generated, by changing the path generation policy or adjusting the margin around static objects.** + +## Limitations + +Here are some notable limitations: + +- If parked vehicles exist in front of, behind, or on both sides of the shoulder, and it's not possible to maintain a specified distance from them, a stopping path will be generated, leading to a potential deadlock. +- In the default settings of the behavior_path_planner, an avoidance module operates in a later stage and attempts to avoid objects. However, it is not guaranteed that the start_planner module will output a path that can successfully navigate around obstacles. This means that if an unavoidable path is generated, it could result in a deadlock. +- The performance of safety check relies on the accuracy of the predicted paths generated by the map_based_prediction node. It's important to note that, currently, predicted paths that consider acceleration are not generated, and paths for low-speed objects may not be accurately produced, which requires caution. +- Given the current specifications of the rule-based algorithms, there is a trade-off between the safety of a path and its naturalness to humans. While it's possible to adjust parameters to manage this trade-off, improvements are necessary to better reconcile these aspects. + +## Start/End Conditions + +### **Start Conditions** + +The `StartPlannerModule` is designed to initiate its execution based on specific criteria evaluated by the `isExecutionRequested` function. The module will **not** start under the following conditions: + +1. **Start pose on the middle of the road**: The module will not initiate if the start pose of the vehicle is determined to be in the middle of the road. This ensures the planner starts from a roadside position. + +2. **Vehicle is far from start position**: If the vehicle is far from the start position, the module will not execute. This prevents redundant execution when the new goal is given. + +3. **Vehicle reached goal**: The module will not start if the vehicle has already reached its goal position, avoiding unnecessary execution when the destination is attained. + +4. **Vehicle in motion**: If the vehicle is still moving, the module will defer starting. This ensures that planning occurs from a stable, stationary state for safety. + +5. **Goal behind in same route segment**: The module will not initiate if the goal position is behind the ego vehicle within the same route segment. This condition is checked to avoid complications with planning routes that require the vehicle to move backward on its current path, which is currently not supported. + +### **End Conditions** + +The `StartPlannerModule` terminates when specific conditions are met, depending on the type of planner being used. The `canTransitSuccessState` function determines whether the module should transition to the success state based on the following criteria: + +#### When the Freespace Planner is active + +- If the end point of the freespace path is reached, the module transitions to the success state. + +#### When any other type of planner is active + +The transition to the success state is determined by the following conditions: + +- If a reverse path is being generated or the search for a pull out path fails: + - The module does not transition to the success state. +- If the end point of the pull out path's shift section is reached: + - The module transitions to the success state. + +The flowchart below illustrates the decision-making process in the `canTransitSuccessState` function: + +```plantuml +@startuml +@startuml +skinparam ActivityBackgroundColor #white +skinparam ActivityBorderColor #black +skinparam ActivityBorderThickness 1 +skinparam ActivityArrowColor #black +skinparam ActivityArrowThickness 1 +skinparam ActivityStartColor #black +skinparam ActivityEndColor #black +skinparam ActivityDiamondBackgroundColor #white +skinparam ActivityDiamondBorderColor #black +skinparam ActivityDiamondFontColor #black +partition canTransitSuccessState() { +start +if (planner type is FREESPACE?) then (yes) +if (Has reached freespace end?) then (yes) +#FF006C:true; +stop +else (no) +:false; +stop +endif +else (no) +if (driving is forward?) then (yes) +if (pull out path is found?) then (yes) +if (Has reached pull out end?) then (yes) +#FF006C:true; +stop +else (no) +:false; +stop +endif +else (no) +:false; +stop +endif +else (no) +:false; +stop +endif +endif +} +@enduml +``` + +## Concept of safety assurance + +The approach to collision safety is divided into two main components: generating paths that consider static information, and detecting collisions with dynamic obstacles to ensure the safety of the generated paths. + +### 1. Generating path with static information + +- **Path deviation checks**: This ensures that the path remains within the designated lanelets. By default, this feature is active, but it can be deactivated if necessary. + +- **Static obstacle clearance from the path**: This involves verifying that a sufficient margin around static obstacles is maintained. The process includes creating a vehicle-sized footprint from the current position to the pull-out endpoint, which can be adjusted via parameters. The distance to static obstacle polygons is then calculated. If this distance is below a specified threshold, the path is deemed unsafe. Threshold levels (e.g., [2.0, 1.0, 0.5, 0.1]) can be configured, and the system searches for paths that meet the highest possible threshold based on a set search priority explained in following section, ensuring the selection of the safe path based on the policy. If no path meets the minimum threshold, it's determined that no safe path is available. + +- **Clearance from stationary objects**: Maintaining an adequate distance from stationary objects positioned in front of and behind the vehicle is imperative for safety. Despite the path and stationary objects having a confirmed margin, the path is deemed unsafe if the distance from the shift start position to a front stationary object falls below `collision_check_margin_from_front_object` meters, or if the distance to a rear stationary object is shorter than `back_objects_collision_check_margin` meters. + + - Why is a margin from the front object necessary? + Consider a scenario in a "geometric pull out path" where the clearance from the path to a static obstacle is minimal, and there is a stopped vehicle ahead. In this case, although the path may meet safety standards and thus be generated, a concurrently operating avoidance module might deem it impossible to avoid the obstacle, potentially leading to vehicle deadlock. To ensure there is enough distance for avoidance maneuvers, the distance to the front obstacle is assessed. Increasing this parameter can prevent immobilization within the avoidance module but may also lead to the frequent generation of backward paths or geometric pull out path, resulting in paths that may seem unnatural to humans. + + - Why is a margin from the rear object necessary? + For objects ahead, another behavior module can intervene, allowing the path to overwrite itself through an avoidance plan, even if the clearance from the path to a static obstacle is minimal, thus maintaining a safe distance from static obstacles. However, for objects behind the vehicle, it is impossible for other behavior modules other than the start_planner to alter the path to secure a margin, potentially leading to a deadlock by an action module like "obstacle_cruise_planner" and subsequent immobilization. Therefore, a margin is set for stationary objects at the rear. + +Here's the expression of the steps start pose searching steps, considering the `collision_check_margins` is set at [2.0, 1.0, 0.5, 0.1] as example. The process is as follows: + +1. **Generating start pose candidates** + + - Set the current position of the vehicle as the base point. + - Determine the area of consideration behind the vehicle up to the `max_back_distance`. + - Generate candidate points for the start pose in the backward direction at intervals defined by `backward_search_resolution`. + - Include the current position as one of the start pose candidates. + + ![start pose candidate](images/start_pose_candidate.drawio.svg){width=1100} + +2. **Starting search at maximum margin** + + - Begin the search with the largest threshold (e.g., 2.0 meters). + - Evaluate each start pose candidate to see if it maintains a margin of more than 2.0 meters. + - Simultaneously, verify that the path generated from that start pose meets other necessary criteria (e.g., path deviation check). + - Following the search priority described later, evaluate each in turn and adopt the start pose if it meets the conditions. + +3. **Repeating search according to threshold levels** + + - If no start pose meeting the conditions is found, lower the threshold to the next level (e.g., 1.0 meter) and repeat the search. + +4. **Continuing the search** + + - Continue the search until a start pose that meets the conditions is found, or the threshold level reaches the minimum value (e.g., 0.1 meter). + - The aim of this process is to find a start pose that not only secures as large a margin as possible but also satisfies the conditions required for the path. + +5. **Generating a stop path** + - If no start pose satisfies the conditions at any threshold level, generate a stop path to ensure safety. + +#### **search priority** + +If a safe path with sufficient clearance for static obstacles cannot be generated forward, a backward search from the vehicle's current position is conducted to locate a suitable start point for a pull out path generation. + +During this backward search, different policies can be applied based on `search_priority` parameters: + +Selecting `efficient_path` focuses on creating a shift pull out path, regardless of how far back the vehicle needs to move. +Opting for `short_back_distance` aims to find a location with the least possible backward movement. + +![priority_order](./images/priority_order.drawio.svg) + +`PriorityOrder` is defined as a vector of pairs, where each element consists of a `size_t` index representing a start pose candidate index, and the planner type. The PriorityOrder vector is processed sequentially from the beginning, meaning that the pairs listed at the top of the vector are given priority in the selection process for pull out path generation. + +##### `efficient_path` + +When `search_priority` is set to `efficient_path` and the preference is for prioritizing `shift_pull_out`, the `PriorityOrder` array is populated in such a way that `shift_pull_out` is grouped together for all start pose candidates before moving on to the next planner type. This prioritization is reflected in the order of the array, with `shift_pull_out` being listed before geometric_pull_out. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 1 | shift_pull_out | +| ... | ... | +| N | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | geometric_pull_out | + +This approach prioritizes trying all candidates with `shift_pull_out` before proceeding to `geometric_pull_out`, which may be efficient in situations where `shift_pull_out` is likely to be appropriate. + +##### `short_back_distance` + +For `search_priority` set to `short_back_distance`, the array alternates between planner types for each start pose candidate, which can minimize the distance the vehicle needs to move backward if the earlier candidates are successful. + +| Index | Planner Type | +| ----- | ------------------ | +| 0 | shift_pull_out | +| 0 | geometric_pull_out | +| 1 | shift_pull_out | +| 1 | geometric_pull_out | +| ... | ... | +| N | shift_pull_out | +| N | geometric_pull_out | + +This ordering is beneficial when the priority is to minimize the backward distance traveled, giving an equal chance for each planner to succeed at the closest possible starting position. + +### 2. Collision detection with dynamic obstacles + +- **Applying RSS in Dynamic Collision Detection**: Collision detection is based on the RSS (Responsibility-Sensitive Safety) model to evaluate if a safe distance is maintained. See [safety check feature explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) + +- **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring the ego vehicle does not impede or hinder the progress of dynamic objects that come from behind it. + +- **Collision response policy**: Should a collision with dynamic objects be detected along the generated path, deactivate module decision is registered if collision detection occurs before departure. If the vehicle has already commenced movement, an attempt to stop will be made, provided it's feasible within the braking constraints and that the rear vehicle can pass through the gap between the ego vehicle and the lane border. + +```plantuml +@startuml +start +:Path Generation; + +if (Collision with dynamic objects detected?) then (yes) + if (Before departure?) then (yes) + :Deactivate module decision is registered; + else (no) + if (Can stop within constraints \n && \n Has sufficient space for rear vehicle to drive?) then (yes) + :Stop; + else (no) + :Continue with caution; + endif + endif +else (no) +endif + +stop +@enduml +``` + +#### **example of safety check performed range for shift pull out** + +Give an example of safety verification range for shift pull out. The safety check is performed from the start of the shift to the end of the shift. And if the vehicle footprint does not leave enough space for a rear vehicle to drive through, the safety check against dynamic objects is disabled. + +
+ ![safety check performed range for shift pull out](images/collision_check_range_shift_pull_out.drawio.svg){width=1100} +
+ +**As a note, no safety check is performed during backward movements. Safety verification commences at the point where the backward motion ceases.** + +## RTC interface + +The system operates distinctly under manual and auto mode, especially concerning the before the departure and interaction with dynamic obstacles. Below are the specific behaviors for each mode: + +### When approval is required? + +#### Forward driving + +- **Start approval required**: Even if a path is generated, approval is required to initiate movement. If a dynamic object poses a risk, such as an approaching vehicle from behind, candidate paths may be displayed, but approval is necessary for departure. + +#### Backward driving + forward driving + +- **Multiple approvals required**: When the planned path includes a backward driving, multiple approvals are needed before starting the reverse and again before restarting forward movement. Before initiating forward movement, the system conducts safety checks against dynamic obstacles. If a detection is detected, approval for departure is necessary. + +This differentiation ensures that the vehicle operates safely by requiring human intervention in manual mode(`enable_rtc` is true) at critical junctures and incorporating automatic safety checks in both modes to prevent unsafe operations in the presence of dynamic obstacles. + +## Design + +```plantuml +@startuml +package start_planner{ + abstract class PullOutPlannerBase { + } + + + class ShiftPullOut { + } + + class GeometricPullOut { + } + + class StartPlannerModule { + } + + struct PullOutPath{} +} + +package utils{ + class PathShifter { + } + + class GeometricParallelParking { + } +} + +' pull out +ShiftPullOut --|> PullOutPlannerBase +GeometricPullOut --|> PullOutPlannerBase + +PathShifter --o ShiftPullOut +GeometricParallelParking --o GeometricPullOut + +PullOutPlannerBase --o StartPlannerModule + +PullOutPath --o PullOutPlannerBase + +@enduml +``` + +## 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_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_margins | [m] | double | Obstacle collision check margins list | [2.0, 1.0, 0.5, 0.1] | +| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | +| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| extra_width_margin_for_rear_obstacle | [m] | double | extra width that is added to the perceived rear obstacle's width when deciding if the rear obstacle can overtake the ego vehicle while it is merging to a lane from the shoulder lane. | 0.5 | +| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | +| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | +| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | +| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | +| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | +| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | +| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | + +### **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 predicted paths of the ego and dynamic objects | 5.0 | +| safety_check_time_resolution | [s] | double | Time resolution for predicted paths of the ego and dynamic objects | 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. + +### **shift pull out** + +This is the most basic method of starting path planning and is used on road lanes and shoulder lanes when there is no particular obstruction. + +Pull out distance is calculated by the speed, lateral deviation, and the lateral jerk. The lateral jerk is searched for among the predetermined minimum and maximum values, and the one that generates a safe path is selected. + +- Generate the road lane centerline and shift it to the current position. +- In the section between merge start and end, path is shifted by a method that is used to generate avoidance path (four segmental constant jerk polynomials) +- Combine this path with center line of road lane + +![shift_pull_out](./images/shift_pull_out.drawio.svg) + +[shift pull out video](https://user-images.githubusercontent.com/39142679/187872468-6d5057ee-e039-499b-afc7-fe0dc8052a6b.mp4) + +#### parameters for shift pull out + +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | +| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true | +| allow_check_shift_path_lane_departure_override | [-] | bool | flag to override/cancel lane departure check if the ego vehicle's starting pose is already out of lane | false | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| 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 | + +### **geometric pull out** + +Generate two arc paths with discontinuous curvature. Ego-vehicle stops once in the middle of the path to control the steer on the spot. +See also [[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) for details of the algorithm. + +![geometric_pull_out](./images/geometric_pull_out.drawio.svg) + +[geometric pull out video](https://user-images.githubusercontent.com/39142679/181024707-3e7ca5ee-62de-4334-b9e9-ded313de1ea1.mp4) + +#### 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 | +| lane_departure_check_expansion_margin | [m] | double | margin to expand the ego vehicle footprint when doing lane departure checks | 0.0 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | + +## **backward pull out start point search** + +If a safe path cannot be generated from the current position, search backwards for a pull out start point at regular intervals(default: `2.0`). + +![pull_out_after_back](./images/pull_out_after_back.drawio.svg) + +[pull out after backward driving video](https://user-images.githubusercontent.com/39142679/181025149-8fb9fb51-9b8f-45c4-af75-27572f4fba78.mp4) + +### **parameters for backward pull out start point search** + +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | +| enable_back | [-] | bool | flag whether to search backward for start_point | true | +| search_priority | [-] | string | In the case of `efficient_path`, use efficient paths even if the back distance is longer. In case of `short_back_distance`, use a path with as short a back distance | efficient_path | +| max_back_distance | [m] | double | maximum back distance | 30.0 | +| 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 | If distance from shift start pose to end of shoulder lane is less than this value, this start pose candidate is ignored | 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](../autoware_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](../autoware_freespace_planner/README.md) for other parameters. diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml similarity index 100% rename from planning/behavior_path_start_planner_module/config/start_planner.param.yaml rename to planning/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml diff --git a/planning/behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/freespace_pull_out.png b/planning/autoware_behavior_path_start_planner_module/images/freespace_pull_out.png similarity index 100% rename from planning/behavior_path_start_planner_module/images/freespace_pull_out.png rename to planning/autoware_behavior_path_start_planner_module/images/freespace_pull_out.png diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/priority_order.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/priority_order.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/priority_order.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/start_planner_example.png b/planning/autoware_behavior_path_start_planner_module/images/start_planner_example.png similarity index 100% rename from planning/behavior_path_start_planner_module/images/start_planner_example.png rename to planning/autoware_behavior_path_start_planner_module/images/start_planner_example.png diff --git a/planning/behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp new file mode 100644 index 0000000000000..15ef4f7a000de --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp @@ -0,0 +1,137 @@ + +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ + +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ + +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +using autoware_perception_msgs::msg::PredictedObjects; + +using autoware::freespace_planning_algorithms::AstarParam; +using autoware::freespace_planning_algorithms::PlannerCommonParam; +using autoware::freespace_planning_algorithms::RRTStarParam; + +struct StartPlannerDebugData +{ + // filtered objects + PredictedObjects filtered_objects; + TargetObjectsOnLane target_objects_on_lane; + std::vector ego_predicted_path; + // collision check debug map + CollisionCheckDebugMap collision_check; + lanelet::ConstLanelets departure_check_lanes; + + Pose refined_start_pose; + std::vector start_pose_candidates; + size_t selected_start_pose_candidate_index; + double margin_for_start_pose_candidate; +}; + +struct StartPlannerParameters +{ + double th_arrived_distance{0.0}; + double th_stopped_velocity{0.0}; + double th_stopped_time{0.0}; + double prepare_time_before_start{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 extra_width_margin_for_rear_obstacle{0.0}; + std::vector collision_check_margins{}; + double collision_check_margin_from_front_object{0.0}; + double th_moving_object_velocity{0.0}; + autoware::behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck + object_types_to_check_for_path_generation{}; + double center_line_path_interval{0.0}; + double lane_departure_check_expansion_margin{0.0}; + + // shift pull out + bool enable_shift_pull_out{false}; + bool check_shift_path_lane_departure{false}; + bool allow_check_shift_path_lane_departure_override{false}; + double shift_collision_check_distance_from_end{0.0}; + 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}; + double maximum_longitudinal_deviation{0.0}; + // geometric pull out + bool enable_geometric_pull_out{false}; + double geometric_collision_check_distance_from_end; + 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{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{false}; + std::string freespace_planner_algorithm; + 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{}; + + // surround moving obstacle check + double search_radius{0.0}; + double th_moving_obstacle_velocity{0.0}; + autoware::behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck + surround_moving_obstacles_type_to_check{}; + + bool print_debug_info{false}; +}; + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp new file mode 100644 index 0000000000000..58b9f2d1aa428 --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp @@ -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. + +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ + +#include "autoware_behavior_path_start_planner_module/data_structs.hpp" + +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::StartPlannerDebugData; + +void updateSafetyCheckDebugData( + StartPlannerDebugData & 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; +} + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp new file mode 100644 index 0000000000000..7e522f8e7ce9d --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp @@ -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. + +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ + +#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" + +#include +#include +#include + +#include + +#include + +namespace autoware::behavior_path_planner +{ +using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::RRTStar; + +class FreespacePullOut : public PullOutPlannerBase +{ +public: + FreespacePullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + + PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } + + std::optional plan(const Pose & start_pose, const Pose & end_pose) override; + +protected: + std::unique_ptr planner_; + double velocity_; + bool use_back_; +}; +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp new file mode 100644 index 0000000000000..43e4654fd7efd --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ + +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" + +#include + +#include + +#include + +namespace autoware::behavior_path_planner +{ +class GeometricPullOut : public PullOutPlannerBase +{ +public: + explicit GeometricPullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const std::shared_ptr + lane_departure_checker); + + PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; + std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; + + GeometricParallelParking planner_; + ParallelParkingParameters parallel_parking_parameters_; + std::shared_ptr lane_departure_checker_; +}; +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp new file mode 100644 index 0000000000000..e2634c7feb34a --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp @@ -0,0 +1,56 @@ +// 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 AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ + +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_behavior_path_start_planner_module/start_planner_module.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ + +class StartPlannerModuleManager : public SceneModuleManagerInterface +{ +public: + StartPlannerModuleManager() : SceneModuleManagerInterface{"start_planner"} {} + + void init(rclcpp::Node * node) override; + + std::unique_ptr createNewSceneModuleInstance() override + { + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); + } + + void updateModuleParams(const std::vector & parameters) override; + + bool isSimultaneousExecutableAsApprovedModule() const override; + + bool isSimultaneousExecutableAsCandidateModule() const override; + +private: + std::shared_ptr parameters_; +}; + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp new file mode 100644 index 0000000000000..d0a764b7b3f1a --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp @@ -0,0 +1,37 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ + +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ +using tier4_planning_msgs::msg::PathWithLaneId; +struct PullOutPath +{ + 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{}; +}; +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp similarity index 76% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp index e49440894f901..c711aac59b141 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -12,21 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_start_planner_module/data_structs.hpp" -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_start_planner_module/data_structs.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; @@ -45,7 +45,7 @@ class PullOutPlannerBase public: explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters) { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } @@ -65,7 +65,7 @@ class PullOutPlannerBase protected: bool isPullOutPathCollided( - behavior_path_planner::PullOutPath & pull_out_path, + autoware::behavior_path_planner::PullOutPath & pull_out_path, double collision_check_distance_from_end) const { // check for collisions @@ -86,7 +86,7 @@ class PullOutPlannerBase pull_out_lane_stop_objects, parameters_.object_types_to_check_for_path_generation); const auto collision_check_section_path = - behavior_path_planner::start_planner_utils::extractCollisionCheckSection( + autoware::behavior_path_planner::start_planner_utils::extractCollisionCheckSection( pull_out_path, collision_check_distance_from_end); if (!collision_check_section_path) return true; @@ -95,11 +95,11 @@ class PullOutPlannerBase collision_check_margin_); }; std::shared_ptr planner_data_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; StartPlannerParameters parameters_; double collision_check_margin_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp similarity index 76% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp index 3bded7ee99fbb..b3170dca49f76 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include +#include #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; class ShiftPullOut : public PullOutPlannerBase { @@ -59,6 +59,6 @@ class ShiftPullOut : public PullOutPlannerBase const double lon_acc, const double shift_time, const double shift_length, const double max_curvature, const double min_distance) const; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp similarity index 82% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp index a89b1e5e31c56..1358420d83abd 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp @@ -12,24 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ - -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/data_structs.hpp" -#include "behavior_path_start_planner_module/freespace_pull_out.hpp" -#include "behavior_path_start_planner_module/geometric_pull_out.hpp" -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/shift_pull_out.hpp" - -#include -#include -#include +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ + +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_start_planner_module/data_structs.hpp" +#include "autoware_behavior_path_start_planner_module/freespace_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/geometric_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/shift_pull_out.hpp" + +#include +#include +#include #include @@ -44,15 +44,15 @@ #include #include -namespace behavior_path_planner +namespace autoware::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 autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +using autoware::lane_departure_checker::LaneDepartureChecker; using geometry_msgs::msg::PoseArray; -using lane_departure_checker::LaneDepartureChecker; using PriorityOrder = std::vector>>; struct PullOutStatus @@ -201,7 +201,7 @@ class StartPlannerModule : public SceneModuleInterface bool requiresDynamicObjectsCollisionDetection() const; uint16_t getSteeringFactorDirection( - const behavior_path_planner::BehaviorModuleOutput & output) const + const autoware::behavior_path_planner::BehaviorModuleOutput & output) const { switch (output.turn_signal_info.turn_signal.command) { case TurnIndicatorsCommand::ENABLE_LEFT: @@ -242,20 +242,20 @@ class StartPlannerModule : public SceneModuleInterface const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); PathWithLaneId extractCollisionCheckSection( - const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type); + const PullOutPath & path, const autoware::behavior_path_planner::PlannerType & planner_type); void updateStatusWithCurrentPath( - const behavior_path_planner::PullOutPath & path, const Pose & start_pose, - const behavior_path_planner::PlannerType & planner_type); + const autoware::behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const autoware::behavior_path_planner::PlannerType & planner_type); void updateStatusWithNextPath( - const behavior_path_planner::PullOutPath & path, const Pose & start_pose, - const behavior_path_planner::PlannerType & planner_type); + const autoware::behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const autoware::behavior_path_planner::PlannerType & planner_type); void updateStatusIfNoSafePathFound(); 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_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::vector> start_planners_; PullOutStatus status_; @@ -334,6 +334,6 @@ class StartPlannerModule : public SceneModuleInterface void setDebugData(); void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp new file mode 100644 index 0000000000000..0cbb0fd504c4b --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp @@ -0,0 +1,58 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ + +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace autoware::behavior_path_planner::start_planner_utils +{ +using autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using autoware::route_handler::RouteHandler; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::PathWithLaneId; + +PathWithLaneId getBackwardPath( + const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, + const Pose & current_pose, const Pose & backed_pose, const double velocity); +lanelet::ConstLanelets getPullOutLanes( + const std::shared_ptr & planner_data, const double backward_length); +Pose getBackedPose( + const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); +std::optional extractCollisionCheckSection( + const PullOutPath & path, const double collision_check_distance_from_end); +} // namespace autoware::behavior_path_planner::start_planner_utils + +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/autoware_behavior_path_start_planner_module/package.xml b/planning/autoware_behavior_path_start_planner_module/package.xml new file mode 100644 index 0000000000000..5b2ad9f8dc9c1 --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/package.xml @@ -0,0 +1,40 @@ + + + + autoware_behavior_path_start_planner_module + 0.1.0 + The autoware_behavior_path_start_planner_module package + + Kosuke Takeuchi + Kyoichi Sugahara + Satoshi OTA + Tomoya Kimura + Shumpei Wakabayashi + Tomohito Ando + Mamoru Sobue + Daniel Sanchez + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_path_planner + autoware_behavior_path_planner_common + autoware_rtc_interface + motion_utils + pluginlib + rclcpp + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_path_start_planner_module/plugins.xml b/planning/autoware_behavior_path_start_planner_module/plugins.xml new file mode 100644 index 0000000000000..ee1ad7b7b370c --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/autoware_behavior_path_start_planner_module/src/debug.cpp b/planning/autoware_behavior_path_start_planner_module/src/debug.cpp new file mode 100644 index 0000000000000..1c07d175a3d09 --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/src/debug.cpp @@ -0,0 +1,30 @@ +// 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 "autoware_behavior_path_start_planner_module/debug.hpp" + +namespace autoware::behavior_path_planner +{ + +void updateSafetyCheckDebugData( + StartPlannerDebugData & 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; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp similarity index 85% rename from planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp rename to planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index dd068c3c26cf8..3178e20a5f51d 100644 --- a/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -12,26 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/freespace_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/freespace_pull_out.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { FreespacePullOut::FreespacePullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) : PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} { - freespace_planning_algorithms::VehicleShape vehicle_shape( + autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); if (parameters.freespace_planner_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; @@ -91,8 +91,9 @@ std::optional FreespacePullOut::plan(const Pose & start_pose, const 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); - const auto path_end_info = behavior_path_planner::utils::parking_departure::calcEndArcLength( - s_start, forward_path_length, road_lanes, goal_pose); + const auto path_end_info = + autoware::behavior_path_planner::utils::parking_departure::calcEndArcLength( + s_start, 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; @@ -114,4 +115,4 @@ std::optional FreespacePullOut::plan(const Pose & start_pose, const return pull_out_path; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp similarity index 89% rename from planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp rename to planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 0f7587fdff476..d5e643c59b176 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/geometric_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/geometric_pull_out.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -26,13 +26,14 @@ using lanelet::utils::getArcCoordinates; using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr lane_departure_checker) + const std::shared_ptr + lane_departure_checker) : PullOutPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_(lane_departure_checker) @@ -120,4 +121,4 @@ std::optional GeometricPullOut::plan(const Pose & start_pose, const return output; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/autoware_behavior_path_start_planner_module/src/manager.cpp new file mode 100644 index 0000000000000..0b83c68b22275 --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -0,0 +1,803 @@ +// 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 "autoware_behavior_path_start_planner_module/manager.hpp" + +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +void StartPlannerModuleManager::init(rclcpp::Node * node) +{ + // init manager interface + initInterface(node, {""}); + + StartPlannerParameters p; + + const std::string ns = "start_planner."; + + p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); + p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); + p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.prepare_time_before_start = node->declare_parameter(ns + "prepare_time_before_start"); + 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"); + p.extra_width_margin_for_rear_obstacle = + node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); + p.collision_check_margins = + node->declare_parameter>(ns + "collision_check_margins"); + p.collision_check_margin_from_front_object = + node->declare_parameter(ns + "collision_check_margin_from_front_object"); + p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + { + const std::string ns = "start_planner.object_types_to_check_for_path_generation."; + p.object_types_to_check_for_path_generation.check_car = + node->declare_parameter(ns + "check_car"); + p.object_types_to_check_for_path_generation.check_truck = + node->declare_parameter(ns + "check_truck"); + p.object_types_to_check_for_path_generation.check_bus = + node->declare_parameter(ns + "check_bus"); + p.object_types_to_check_for_path_generation.check_trailer = + node->declare_parameter(ns + "check_trailer"); + p.object_types_to_check_for_path_generation.check_unknown = + node->declare_parameter(ns + "check_unknown"); + p.object_types_to_check_for_path_generation.check_bicycle = + node->declare_parameter(ns + "check_bicycle"); + p.object_types_to_check_for_path_generation.check_motorcycle = + node->declare_parameter(ns + "check_motorcycle"); + p.object_types_to_check_for_path_generation.check_pedestrian = + node->declare_parameter(ns + "check_pedestrian"); + } + 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 = + node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); + p.shift_collision_check_distance_from_end = + node->declare_parameter(ns + "shift_collision_check_distance_from_end"); + p.minimum_shift_pull_out_distance = + node->declare_parameter(ns + "minimum_shift_pull_out_distance"); + p.lateral_acceleration_sampling_num = + node->declare_parameter(ns + "lateral_acceleration_sampling_num"); + p.lateral_jerk = node->declare_parameter(ns + "lateral_jerk"); + p.maximum_lateral_acc = node->declare_parameter(ns + "maximum_lateral_acc"); + p.minimum_lateral_acc = node->declare_parameter(ns + "minimum_lateral_acc"); + p.maximum_curvature = node->declare_parameter(ns + "maximum_curvature"); + p.deceleration_interval = node->declare_parameter(ns + "deceleration_interval"); + p.maximum_longitudinal_deviation = + node->declare_parameter(ns + "maximum_longitudinal_deviation"); + // geometric pull out + p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); + p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); + p.parallel_parking_parameters.pull_out_velocity = + node->declare_parameter(ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_path_interval = + node->declare_parameter(ns + "arc_path_interval"); + p.parallel_parking_parameters.pull_out_lane_departure_margin = + node->declare_parameter(ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + node->declare_parameter(ns + "lane_departure_check_expansion_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" + p.enable_back = node->declare_parameter(ns + "enable_back"); + p.backward_velocity = node->declare_parameter(ns + "backward_velocity"); + p.max_back_distance = node->declare_parameter(ns + "max_back_distance"); + p.backward_search_resolution = node->declare_parameter(ns + "backward_search_resolution"); + p.backward_path_update_duration = + node->declare_parameter(ns + "backward_path_update_duration"); + p.ignore_distance_from_lane_end = + node->declare_parameter(ns + "ignore_distance_from_lane_end"); + // freespace planner general params + { + const std::string ns = "start_planner.freespace_planner."; + p.enable_freespace_planner = node->declare_parameter(ns + "enable_freespace_planner"); + p.freespace_planner_algorithm = + node->declare_parameter(ns + "freespace_planner_algorithm"); + p.end_pose_search_start_distance = + node->declare_parameter(ns + "end_pose_search_start_distance"); + p.end_pose_search_end_distance = + node->declare_parameter(ns + "end_pose_search_end_distance"); + p.end_pose_search_interval = node->declare_parameter(ns + "end_pose_search_interval"); + p.freespace_planner_velocity = node->declare_parameter(ns + "velocity"); + p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); + p.freespace_planner_common_parameters.time_limit = + node->declare_parameter(ns + "time_limit"); + p.freespace_planner_common_parameters.minimum_turning_radius = + node->declare_parameter(ns + "minimum_turning_radius"); + p.freespace_planner_common_parameters.maximum_turning_radius = + node->declare_parameter(ns + "maximum_turning_radius"); + p.freespace_planner_common_parameters.turning_radius_size = + node->declare_parameter(ns + "turning_radius_size"); + p.freespace_planner_common_parameters.maximum_turning_radius = std::max( + p.freespace_planner_common_parameters.maximum_turning_radius, + p.freespace_planner_common_parameters.minimum_turning_radius); + p.freespace_planner_common_parameters.turning_radius_size = + std::max(p.freespace_planner_common_parameters.turning_radius_size, 1); + } + // freespace planner search config + { + const std::string ns = "start_planner.freespace_planner.search_configs."; + p.freespace_planner_common_parameters.theta_size = + node->declare_parameter(ns + "theta_size"); + p.freespace_planner_common_parameters.angle_goal_range = + node->declare_parameter(ns + "angle_goal_range"); + p.freespace_planner_common_parameters.curve_weight = + node->declare_parameter(ns + "curve_weight"); + p.freespace_planner_common_parameters.reverse_weight = + node->declare_parameter(ns + "reverse_weight"); + p.freespace_planner_common_parameters.lateral_goal_range = + node->declare_parameter(ns + "lateral_goal_range"); + p.freespace_planner_common_parameters.longitudinal_goal_range = + node->declare_parameter(ns + "longitudinal_goal_range"); + } + // freespace planner costmap configs + { + const std::string ns = "start_planner.freespace_planner.costmap_configs."; + p.freespace_planner_common_parameters.obstacle_threshold = + node->declare_parameter(ns + "obstacle_threshold"); + } + // freespace planner astar + { + const std::string ns = "start_planner.freespace_planner.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"); + p.astar_parameters.distance_heuristic_weight = + node->declare_parameter(ns + "distance_heuristic_weight"); + } + // freespace planner rrtstar + { + const std::string ns = "start_planner.freespace_planner.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"); + p.rrt_star_parameters.max_planning_time = + node->declare_parameter(ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); + 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"); + } + + const std::string base_ns = "start_planner.path_safety_check."; + + // EgoPredictedPath + const 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 + "min_acceleration"); + 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 + const 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 + const 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 + const 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 + const 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 + const 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"); + } + + // surround moving obstacle check + std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; + { + p.search_radius = + node->declare_parameter(surround_moving_obstacle_check_ns + "search_radius"); + p.th_moving_obstacle_velocity = node->declare_parameter( + surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); + // ObjectTypesToCheck + std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + { + p.surround_moving_obstacles_type_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.surround_moving_obstacles_type_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.surround_moving_obstacles_type_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.surround_moving_obstacles_type_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.surround_moving_obstacles_type_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.surround_moving_obstacles_type_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.surround_moving_obstacles_type_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.surround_moving_obstacles_type_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + } + + // debug + std::string debug_ns = ns + "debug."; + { + p.print_debug_info = node->declare_parameter(debug_ns + "print_debug_info"); + } + + // validation of parameters + if (p.lateral_acceleration_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + node->get_logger().get_child(name()), + "lateral_acceleration_sampling_num must be positive integer. Given parameter: " + << p.lateral_acceleration_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + + parameters_ = std::make_shared(p); +} + +void StartPlannerModuleManager::updateModuleParams( + [[maybe_unused]] const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + const std::string ns = "start_planner."; + + { + updateParam(parameters, ns + "th_arrived_distance", p->th_arrived_distance); + updateParam(parameters, ns + "th_stopped_velocity", p->th_stopped_velocity); + updateParam(parameters, ns + "th_stopped_time", p->th_stopped_time); + updateParam(parameters, ns + "prepare_time_before_start", p->prepare_time_before_start); + updateParam( + parameters, ns + "th_turn_signal_on_lateral_offset", p->th_turn_signal_on_lateral_offset); + updateParam( + parameters, ns + "th_distance_to_middle_of_the_road", p->th_distance_to_middle_of_the_road); + updateParam( + parameters, ns + "intersection_search_length", p->intersection_search_length); + updateParam( + parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection", + p->length_ratio_for_turn_signal_deactivation_near_intersection); + updateParam( + parameters, ns + "extra_width_margin_for_rear_obstacle", + p->extra_width_margin_for_rear_obstacle); + + updateParam>( + parameters, ns + "collision_check_margins", p->collision_check_margins); + updateParam( + parameters, ns + "collision_check_margin_from_front_object", + p->collision_check_margin_from_front_object); + updateParam(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity); + const std::string obj_types_ns = ns + "object_types_to_check_for_path_generation."; + { + updateParam( + parameters, obj_types_ns + "check_car", + p->object_types_to_check_for_path_generation.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->object_types_to_check_for_path_generation.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->object_types_to_check_for_path_generation.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->object_types_to_check_for_path_generation.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->object_types_to_check_for_path_generation.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->object_types_to_check_for_path_generation.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->object_types_to_check_for_path_generation.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->object_types_to_check_for_path_generation.check_pedestrian); + } + updateParam(parameters, ns + "center_line_path_interval", p->center_line_path_interval); + updateParam(parameters, ns + "enable_shift_pull_out", p->enable_shift_pull_out); + updateParam( + parameters, ns + "shift_collision_check_distance_from_end", + p->shift_collision_check_distance_from_end); + updateParam( + parameters, ns + "minimum_shift_pull_out_distance", p->minimum_shift_pull_out_distance); + updateParam( + parameters, ns + "lateral_acceleration_sampling_num", p->lateral_acceleration_sampling_num); + updateParam(parameters, ns + "lateral_jerk", p->lateral_jerk); + updateParam(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc); + updateParam(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc); + updateParam(parameters, ns + "maximum_curvature", p->maximum_curvature); + updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); + updateParam( + parameters, ns + "maximum_longitudinal_deviation", p->maximum_longitudinal_deviation); + updateParam(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); + updateParam(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path); + updateParam( + parameters, ns + "arc_path_interval", p->parallel_parking_parameters.pull_out_path_interval); + updateParam( + parameters, ns + "lane_departure_margin", + p->parallel_parking_parameters.pull_out_lane_departure_margin); + updateParam( + parameters, ns + "lane_departure_check_expansion_margin", + p->lane_departure_check_expansion_margin); + updateParam( + parameters, ns + "pull_out_max_steer_angle", + p->parallel_parking_parameters.pull_out_max_steer_angle); + updateParam(parameters, ns + "enable_back", p->enable_back); + updateParam(parameters, ns + "backward_velocity", p->backward_velocity); + updateParam( + parameters, ns + "geometric_pull_out_velocity", + p->parallel_parking_parameters.pull_out_velocity); + updateParam( + parameters, ns + "geometric_collision_check_distance_from_end", + p->geometric_collision_check_distance_from_end); + updateParam( + parameters, ns + "check_shift_path_lane_departure", p->check_shift_path_lane_departure); + updateParam( + parameters, ns + "allow_check_shift_path_lane_departure_override", + p->allow_check_shift_path_lane_departure_override); + updateParam(parameters, ns + "search_priority", p->search_priority); + updateParam(parameters, ns + "max_back_distance", p->max_back_distance); + updateParam( + parameters, ns + "backward_search_resolution", p->backward_search_resolution); + updateParam( + parameters, ns + "backward_path_update_duration", p->backward_path_update_duration); + updateParam( + parameters, ns + "ignore_distance_from_lane_end", p->ignore_distance_from_lane_end); + } + { + const std::string ns = "start_planner.freespace_planner."; + + updateParam(parameters, ns + "enable_freespace_planner", p->enable_freespace_planner); + updateParam( + parameters, ns + "freespace_planner_algorithm", p->freespace_planner_algorithm); + updateParam( + parameters, ns + "end_pose_search_start_distance", p->end_pose_search_start_distance); + updateParam( + parameters, ns + "end_pose_search_end_distance", p->end_pose_search_end_distance); + updateParam(parameters, ns + "end_pose_search_interval", p->end_pose_search_interval); + updateParam(parameters, ns + "velocity", p->freespace_planner_velocity); + updateParam(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); + updateParam( + parameters, ns + "time_limit", p->freespace_planner_common_parameters.time_limit); + updateParam( + parameters, ns + "minimum_turning_radius", + p->freespace_planner_common_parameters.minimum_turning_radius); + updateParam( + parameters, ns + "maximum_turning_radius", + p->freespace_planner_common_parameters.maximum_turning_radius); + updateParam( + parameters, ns + "turning_radius_size", + p->freespace_planner_common_parameters.turning_radius_size); + p->freespace_planner_common_parameters.maximum_turning_radius = std::max( + p->freespace_planner_common_parameters.maximum_turning_radius, + p->freespace_planner_common_parameters.minimum_turning_radius); + p->freespace_planner_common_parameters.turning_radius_size = + std::max(p->freespace_planner_common_parameters.turning_radius_size, 1); + } + { + const std::string ns = "start_planner.freespace_planner.search_configs."; + + updateParam( + parameters, ns + "theta_size", p->freespace_planner_common_parameters.theta_size); + updateParam( + parameters, ns + "angle_goal_range", p->freespace_planner_common_parameters.angle_goal_range); + updateParam( + parameters, ns + "curve_weight", p->freespace_planner_common_parameters.curve_weight); + updateParam( + parameters, ns + "reverse_weight", p->freespace_planner_common_parameters.reverse_weight); + updateParam( + parameters, ns + "lateral_goal_range", + p->freespace_planner_common_parameters.lateral_goal_range); + updateParam( + parameters, ns + "longitudinal_goal_range", + p->freespace_planner_common_parameters.longitudinal_goal_range); + } + + { + const std::string ns = "start_planner.freespace_planner.costmap_configs."; + + updateParam( + parameters, ns + "obstacle_threshold", + p->freespace_planner_common_parameters.obstacle_threshold); + } + + { + const std::string ns = "start_planner.freespace_planner.astar."; + + updateParam(parameters, ns + "use_back", p->astar_parameters.use_back); + updateParam( + parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions); + updateParam( + parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight); + } + { + const std::string ns = "start_planner.freespace_planner.rrtstar."; + + updateParam(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); + updateParam( + parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling); + updateParam( + parameters, ns + "max_planning_time", p->rrt_star_parameters.max_planning_time); + updateParam(parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius); + updateParam(parameters, ns + "margin", p->rrt_star_parameters.margin); + } + + { + updateParam( + parameters, ns + "stop_condition.maximum_deceleration_for_stop", + p->maximum_deceleration_for_stop); + updateParam( + parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); + } + + const std::string base_ns = "start_planner.path_safety_check."; + const std::string ego_path_ns = base_ns + "ego_predicted_path."; + + { + updateParam( + parameters, ego_path_ns + "min_velocity", p->ego_predicted_path_params.min_velocity); + updateParam( + parameters, ego_path_ns + "min_acceleration", p->ego_predicted_path_params.acceleration); + updateParam( + parameters, ego_path_ns + "time_horizon_for_front_object", + p->ego_predicted_path_params.time_horizon_for_front_object); + updateParam( + parameters, ego_path_ns + "time_horizon_for_rear_object", + p->ego_predicted_path_params.time_horizon_for_rear_object); + updateParam( + parameters, ego_path_ns + "time_resolution", p->ego_predicted_path_params.time_resolution); + updateParam( + parameters, ego_path_ns + "delay_until_departure", + p->ego_predicted_path_params.delay_until_departure); + } + + const std::string obj_filter_ns = base_ns + "target_filtering."; + + { + updateParam( + parameters, obj_filter_ns + "safety_check_time_horizon", + p->objects_filtering_params.safety_check_time_horizon); + updateParam( + parameters, obj_filter_ns + "safety_check_time_resolution", + p->objects_filtering_params.safety_check_time_resolution); + updateParam( + parameters, obj_filter_ns + "object_check_forward_distance", + p->objects_filtering_params.object_check_forward_distance); + updateParam( + parameters, obj_filter_ns + "object_check_backward_distance", + p->objects_filtering_params.object_check_backward_distance); + updateParam( + parameters, obj_filter_ns + "ignore_object_velocity_threshold", + p->objects_filtering_params.ignore_object_velocity_threshold); + updateParam( + parameters, obj_filter_ns + "include_opposite_lane", + p->objects_filtering_params.include_opposite_lane); + updateParam( + parameters, obj_filter_ns + "invert_opposite_lane", + p->objects_filtering_params.invert_opposite_lane); + updateParam( + parameters, obj_filter_ns + "check_all_predicted_path", + p->objects_filtering_params.check_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_all_predicted_path", + p->objects_filtering_params.use_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_predicted_path_outside_lanelet", + p->objects_filtering_params.use_predicted_path_outside_lanelet); + } + + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + + { + updateParam( + parameters, obj_types_ns + "check_car", + p->objects_filtering_params.object_types_to_check.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->objects_filtering_params.object_types_to_check.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->objects_filtering_params.object_types_to_check.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->objects_filtering_params.object_types_to_check.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->objects_filtering_params.object_types_to_check.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->objects_filtering_params.object_types_to_check.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->objects_filtering_params.object_types_to_check.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->objects_filtering_params.object_types_to_check.check_pedestrian); + } + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + + { + updateParam( + parameters, obj_lane_ns + "check_current_lane", + p->objects_filtering_params.object_lane_configuration.check_current_lane); + updateParam( + parameters, obj_lane_ns + "check_right_side_lane", + p->objects_filtering_params.object_lane_configuration.check_right_lane); + updateParam( + parameters, obj_lane_ns + "check_left_side_lane", + p->objects_filtering_params.object_lane_configuration.check_left_lane); + updateParam( + parameters, obj_lane_ns + "check_shoulder_lane", + p->objects_filtering_params.object_lane_configuration.check_shoulder_lane); + updateParam( + parameters, obj_lane_ns + "check_other_lane", + p->objects_filtering_params.object_lane_configuration.check_other_lane); + } + const std::string safety_check_ns = base_ns + "safety_check_params."; + { + updateParam( + parameters, safety_check_ns + "enable_safety_check", + p->safety_check_params.enable_safety_check); + updateParam( + parameters, safety_check_ns + "hysteresis_factor_expand_rate", + p->safety_check_params.hysteresis_factor_expand_rate); + updateParam( + parameters, safety_check_ns + "backward_path_length", + p->safety_check_params.backward_path_length); + updateParam( + parameters, safety_check_ns + "forward_path_length", + p->safety_check_params.forward_path_length); + updateParam( + parameters, safety_check_ns + "publish_debug_marker", + p->safety_check_params.publish_debug_marker); + } + const std::string rss_ns = safety_check_ns + "rss_params."; + { + updateParam( + parameters, rss_ns + "rear_vehicle_reaction_time", + p->safety_check_params.rss_params.rear_vehicle_reaction_time); + updateParam( + parameters, rss_ns + "rear_vehicle_safety_time_margin", + p->safety_check_params.rss_params.rear_vehicle_safety_time_margin); + updateParam( + parameters, rss_ns + "lateral_distance_max_threshold", + p->safety_check_params.rss_params.lateral_distance_max_threshold); + updateParam( + parameters, rss_ns + "longitudinal_distance_min_threshold", + p->safety_check_params.rss_params.longitudinal_distance_min_threshold); + updateParam( + parameters, rss_ns + "longitudinal_velocity_delta_time", + p->safety_check_params.rss_params.longitudinal_velocity_delta_time); + } + std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; + { + updateParam( + parameters, surround_moving_obstacle_check_ns + "search_radius", p->search_radius); + updateParam( + parameters, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity", + p->th_moving_obstacle_velocity); + + // ObjectTypesToCheck + std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + { + updateParam( + parameters, obj_types_ns + "check_car", + p->surround_moving_obstacles_type_to_check.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->surround_moving_obstacles_type_to_check.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->surround_moving_obstacles_type_to_check.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->surround_moving_obstacles_type_to_check.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->surround_moving_obstacles_type_to_check.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->surround_moving_obstacles_type_to_check.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->surround_moving_obstacles_type_to_check.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->surround_moving_obstacles_type_to_check.check_pedestrian); + } + } + + std::string debug_ns = ns + "debug."; + { + updateParam(parameters, debug_ns + "print_debug_info", p->print_debug_info); + } + + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); + }); +} + +bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const +{ + if (observers_.empty()) { + return config_.enable_simultaneous_execution_as_approved_module; + } + + const auto checker = [this](const SceneModuleObserver & observer) { + if (observer.expired()) { + return config_.enable_simultaneous_execution_as_approved_module; + } + + 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->isDrivingForward()) { + return false; + } + + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + + return config_.enable_simultaneous_execution_as_approved_module; + }; + + return std::all_of(observers_.begin(), observers_.end(), checker); +} + +bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const +{ + if (observers_.empty()) { + return config_.enable_simultaneous_execution_as_candidate_module; + } + + const auto checker = [this](const SceneModuleObserver & observer) { + if (observer.expired()) { + return config_.enable_simultaneous_execution_as_candidate_module; + } + + 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->isDrivingForward()) { + return false; + } + + // Other modules are not needed when freespace planning + if (start_planner_ptr->isFreespacePlanning()) { + return false; + } + + return config_.enable_simultaneous_execution_as_candidate_module; + }; + + return std::all_of(observers_.begin(), observers_.end(), checker); +} +} // namespace autoware::behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_path_planner::StartPlannerModuleManager, + autoware::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp similarity index 96% rename from planning/behavior_path_start_planner_module/src/shift_pull_out.cpp rename to planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 592f57ed97dcf..ade53acf1e2b1 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/shift_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/shift_pull_out.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" @@ -31,7 +31,7 @@ using lanelet::utils::getArcCoordinates; using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using start_planner_utils::getPullOutLanes; @@ -225,8 +225,9 @@ std::vector ShiftPullOut::calcPullOutPaths( // generate road lane reference path const auto arc_position_start = getArcCoordinates(road_lanes, start_pose); const double s_start = std::max(arc_position_start.length - backward_path_length, 0.0); - const auto path_end_info = behavior_path_planner::utils::parking_departure::calcEndArcLength( - s_start, forward_path_length, road_lanes, goal_pose); + const auto path_end_info = + autoware::behavior_path_planner::utils::parking_departure::calcEndArcLength( + s_start, 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; @@ -416,4 +417,4 @@ double ShiftPullOut::calcBeforeShiftedArcLength( return before_arc_length; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp similarity index 97% rename from planning/behavior_path_start_planner_module/src/start_planner_module.cpp rename to planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 7e1f38f30c378..15225d719e1b4 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/start_planner_module.hpp" - -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_start_planner_module/debug.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/start_planner_module.hpp" + +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_start_planner_module/debug.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -39,8 +39,8 @@ #include #include -using behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; -using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using autoware::behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; +using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; @@ -50,7 +50,7 @@ using tier4_autoware_utils::calcOffsetPose; #define DEBUG_PRINT(...) \ RCLCPP_DEBUG_EXPRESSION(getLogger(), parameters_->print_debug_info, __VA_ARGS__) -namespace behavior_path_planner +namespace autoware::behavior_path_planner { StartPlannerModule::StartPlannerModule( const std::string & name, rclcpp::Node & node, @@ -60,12 +60,12 @@ StartPlannerModule::StartPlannerModule( objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); - lane_departure_checker::Param lane_departure_checker_params; + autoware::lane_departure_checker::Param lane_departure_checker_params; lane_departure_checker_params.footprint_extra_margin = parameters->lane_departure_check_expansion_margin; @@ -478,7 +478,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough() const lanelet::ConstLanelet closest_lanelet_const(closest_lanelet.constData()); // Check backwards just in case the Vehicle behind ego is in a different lanelet constexpr double backwards_length = 200.0; - const auto prev_lanes = behavior_path_planner::utils::getBackwardLanelets( + const auto prev_lanes = autoware::behavior_path_planner::utils::getBackwardLanelets( *route_handler, target_lanes, current_pose, backwards_length); // return all the relevant lanelets lanelet::ConstLanelets relevant_lanelets{closest_lanelet_const}; @@ -658,7 +658,7 @@ BehaviorModuleOutput StartPlannerModule::plan() if (!status_.is_safe_dynamic_objects) { auto current_path = getCurrentPath(); const auto stop_path = - behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( + autoware::behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); @@ -915,8 +915,8 @@ bool StartPlannerModule::findPullOutPath( } void StartPlannerModule::updateStatusWithCurrentPath( - const behavior_path_planner::PullOutPath & path, const Pose & start_pose, - const behavior_path_planner::PlannerType & planner_type) + const autoware::behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const autoware::behavior_path_planner::PlannerType & planner_type) { const std::lock_guard lock(mutex_); status_.driving_forward = true; @@ -927,8 +927,8 @@ void StartPlannerModule::updateStatusWithCurrentPath( } void StartPlannerModule::updateStatusWithNextPath( - const behavior_path_planner::PullOutPath & path, const Pose & start_pose, - const behavior_path_planner::PlannerType & planner_type) + const autoware::behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const autoware::behavior_path_planner::PlannerType & planner_type) { const std::lock_guard lock(mutex_); status_.driving_forward = false; @@ -1371,7 +1371,7 @@ bool StartPlannerModule::isSafePath() const 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( + autoware::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); @@ -1401,7 +1401,7 @@ bool StartPlannerModule::isSafePath() const merged_target_object.insert( merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(), target_objects_on_lane.on_shoulder_lane.end()); - return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( + return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( pull_out_path, ego_predicted_path, merged_target_object, debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, hysteresis_factor); @@ -1831,4 +1831,4 @@ void StartPlannerModule::StartPlannerData::update( main_thread_pull_out_status = pull_out_status_; is_stopped = is_stopped_; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_start_planner_module/src/util.cpp b/planning/autoware_behavior_path_start_planner_module/src/util.cpp new file mode 100644 index 0000000000000..e8197fcf095cc --- /dev/null +++ b/planning/autoware_behavior_path_start_planner_module/src/util.cpp @@ -0,0 +1,142 @@ +// Copyright 2021 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 "autoware_behavior_path_start_planner_module/util.hpp" + +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_path_planner::start_planner_utils +{ +PathWithLaneId getBackwardPath( + const RouteHandler & route_handler, const lanelet::ConstLanelets & shoulder_lanes, + const Pose & current_pose, const Pose & backed_pose, const double velocity) +{ + const auto current_pose_arc_coords = + lanelet::utils::getArcCoordinates(shoulder_lanes, current_pose); + const auto backed_pose_arc_coords = + lanelet::utils::getArcCoordinates(shoulder_lanes, backed_pose); + + const double s_start = backed_pose_arc_coords.length; + const double s_end = current_pose_arc_coords.length; + + PathWithLaneId backward_path; + { + // forward center line path + backward_path = route_handler.getCenterLinePath(shoulder_lanes, s_start, s_end, true); + + // backward center line path + std::reverse(backward_path.points.begin(), backward_path.points.end()); + for (auto & p : backward_path.points) { + p.point.longitudinal_velocity_mps = velocity; + } + backward_path.points.back().point.longitudinal_velocity_mps = 0.0; + + // lateral shift to current_pose + const double lateral_distance_to_shoulder_center = current_pose_arc_coords.distance; + for (size_t i = 0; i < backward_path.points.size(); ++i) { + auto & p = backward_path.points.at(i).point.pose; + p = tier4_autoware_utils::calcOffsetPose(p, 0, lateral_distance_to_shoulder_center, 0); + } + } + + return backward_path; +} + +Pose getBackedPose( + const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance) +{ + Pose backed_pose; + backed_pose = current_pose; + backed_pose.position.x -= std::cos(yaw_shoulder_lane) * back_distance; + backed_pose.position.y -= std::sin(yaw_shoulder_lane) * back_distance; + + return backed_pose; +} + +lanelet::ConstLanelets getPullOutLanes( + const std::shared_ptr & planner_data, const double backward_length) +{ + const double & vehicle_width = planner_data->parameters.vehicle_width; + const auto & route_handler = planner_data->route_handler; + const auto start_pose = planner_data->route_handler->getOriginalStartPose(); + + const auto current_shoulder_lane = route_handler->getPullOutStartLane(start_pose, vehicle_width); + if (current_shoulder_lane) { + // pull out from shoulder lane + return route_handler->getShoulderLaneletSequence(*current_shoulder_lane, start_pose); + } + + // pull out from road lane + return utils::getExtendedCurrentLanes( + planner_data, backward_length, + /*forward_length*/ std::numeric_limits::max(), + /*forward_only_in_route*/ true); +} + +std::optional extractCollisionCheckSection( + const PullOutPath & path, const double collision_check_distance_from_end) +{ + PathWithLaneId full_path; + for (const auto & partial_path : path.partial_paths) { + full_path.points.insert( + full_path.points.end(), partial_path.points.begin(), partial_path.points.end()); + } + + if (full_path.points.empty()) return std::nullopt; + // Find the start index for collision check section based on the shift start pose + const auto shift_start_idx = + motion_utils::findNearestIndex(full_path.points, path.start_pose.position); + + // Find the end index for collision check section based on the end pose and collision check + // distance + const auto collision_check_end_idx = [&]() -> size_t { + const auto end_pose_offset = motion_utils::calcLongitudinalOffsetPose( + full_path.points, path.end_pose.position, collision_check_distance_from_end); + + return end_pose_offset + ? motion_utils::findNearestIndex(full_path.points, end_pose_offset->position) + : full_path.points.size() - 1; // Use the last point if offset pose is not calculable + }(); + + // Extract the collision check section from the full path + PathWithLaneId collision_check_section; + if (shift_start_idx < collision_check_end_idx) { + collision_check_section.points.assign( + full_path.points.begin() + shift_start_idx, + full_path.points.begin() + collision_check_end_idx + 1); + } + + return collision_check_section; +} + +} // namespace autoware::behavior_path_planner::start_planner_utils diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt b/planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt index e646ad29ed42a..359a0a75dd1ca 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(autoware_behavior_path_static_obstacle_avoidance_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(behavior_path_planner plugins.xml) +pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md index 93b57ac3a4831..bc70c1fdd5c6a 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -1,12 +1,16 @@ -# Avoidance design +# Avoidance module for static objects -This is a rule-based path planning module designed for obstacle avoidance. +![fig](./images/purpose/rviz.png) ## Purpose / Role -This module is designed for rule-based avoidance that is easy for developers to design its behavior. It generates avoidance path parameterized by intuitive parameters such as lateral jerk and avoidance distance margin. This makes it possible to pre-define avoidance behavior. +This is a rule-based avoidance module, which is running based on perception output data, HDMap, current path and route. This module is designed for creating avoidance path for static (=stopped) objects in simple situation. On the other hand, this module doesn't support dynamic (=moving) objects for now. -In addition, the approval interface of behavior_path_planner allows external users / modules (e.g. remote operation) to intervene the decision of the vehicle behavior. This function is expected to be used, for example, for remote intervention in emergency situations or gathering information on operator decisions during development. +![fig](./images/purpose/avoidance.png) + +This module has [RTC interface](../autoware_rtc_interface/README.md), and user can select operation mode from MANUAL/AUTO depending on the vehicle sensor performance. If user selects MANUAL mode, this module outputs avoidance path as candidate and waits operator approval. In the case where the sensor/perception performance is not enough and false positive maybe occurs, we recommend to use this module with MANUAL mode in order to prevent unnecessary avoidance maneuver. + +On the other hand, if user selects AUTO mode, this module modifies current following path without operator approval. If the sensor/perception performance is good enough, user can use this module with AUTO mode. ### Limitations @@ -18,264 +22,439 @@ This module executes avoidance over lanes, and the decision requires the lane st ## Inner-workings / Algorithms -The following figure shows a simple explanation of the logic for avoidance path generation. First, target objects are picked up, and shift requests are generated for each object. These shift requests are generated by taking into account the lateral jerk required for avoidance (red lines). Then these requests are merged and the shift points are created on the reference path (blue line). Filtering operations are performed on the shift points such as removing unnecessary shift points (yellow line), and finally a smooth avoidance path is generated by combining Clothoid-like curve primitives (green line). - -![fig1](./images/avoidance_design.fig1.drawio.svg) - -### Flowchart +This module mainly has two parts, target filtering part and path generation part. At first, all objects are filtered by several conditions. In this step, this module checks avoidance feasibility and necessity as well. After that, this module generates avoidance path outline, whom we call **shift line**, based on filtered objects. The shift lines are set into [path shifter](../autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md), which is a library for path generation, to create smooth shift path. Additionally, this module has feature to check non-target objects so that the ego can avoid target object safely. This feature receives generated avoidance path and surround objects and judges current situation. Lastly, this module update current ego behavior. ```plantuml @startuml skinparam monochrome true -title avoidance path planning +title Overall flowchart start -:calcAvoidancePlanningData; +partition updateData() { +partition fillFundamentalData() { +:update fundamental data; note right - - center line + - reference pose - reference path: generated by spline interpolation of centerline path with dense interval - - driving lanelets - - objects + - current lanelets + - drivable bounds + - avoidance start point + calculate the point where the ego should start avoidance maneuver + depending on traffic siganl. + - avoidance return point + calculate the point where the ego should return original lane + depending on traffic siganl and goal position. end note -:calcAvoidanceTargetObjects; +:fillAvoidanceTargetObjects(); note right - Find objects that satisfies all of the following: + This module checks following conditions: - target object type - being stopped - being around the ego-driving lane - being on the edge of the lane + - ... end note -if (Is target object found?) then ( yes) -else (\n no) - if (Does path_shifter already have shift points?) then (no) - :exit avoidance module; - stop - else (yes) - endif -endif - -partition shift-point\ngeneration { +:updateRegisteredObject(); -:calcRawShiftPointsFromObjects; -note right - generate "raw_shift_points" for each target objects. - the shift point contains: - - avoiding shift - - return-to-center shift - with considering the lateral jerk range. -end note +:compensateDetectionLost(); +} -:combineRawShiftPointsWithUniqueCheck; -note right - combine with "already-registered raw_shift_points". +partition fillShiftLine() { +:generate shift line; - The raw_shift_point is saved when the relevant - shift is executed. Here the registered - raw_shift_points are added with the raw_shift_Point - calculated in this step. -end note +:create candidate path; -:addReturnShiftPointFromEgo; +:check candidate path; note right - 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. + This module checks following conditions: + - is there enough distance between surround moving vehicle and ego path to avoid target safely? + - is the path jerky? + - is the path within drivable area? end note +} -:mergeShiftPoints; +partition fillEgoStatus() { +:getCurrentModuleState(); note right -shift points for each objects are merged to consider -overlapping region, etc and new shift points are created -for the current reference trajectory. + This module has following status: + - RUNNING: target object is still remaining. Or, the ego hasn't returned original lane. + - CANCEL: taget obejct has gone. And, the ego hasn't initiated avoidance maneuver. + - SUCCEEDED: the ego finishes avoiding all objects and returns original lane. end note -:trimShiftPoint; +if (canYieldManeuver()) then (yes) +if (Is the avoidance path safe?) then (yes) +else (no) +:transit yield maneuver; note right -modify shift points or remove unnecessary points. -it contains some operations: - - quantize shift length - - trim small shift - - trim similar gradient - - trim momentary return + Check current situation. + This module can transit yield maneuver only when the ego hasn't initiated avoidance maneuver. end note +endif +else (no) - +endif } +} +stop + +start +partition isExecutionRequested() { +if (Is there object that should/can be avoid immediately?) then (yes) +:return true; +stop +else (no) +endif -:findNewShiftPoint; +if (Does it generate new shift lines successfully?) then (yes) +else (no) +:return false; +stop +endif + +if (Is there object that is potentially avoidable?) then (yes) +:return true; note right - If there is a shift point that has a large - deviation from previous planned path, - it is considered as a new shift point. + Sometimes, we meet the situation where there is enough space to avoid + but ego speed is to high to avoid target obejct under lateral jerk constraints. + This module keeps running in this case in order to decelerate ego speed. end note +stop +else (no) +:return false; +endif +} +stop -if (Is the new shift point found?) then ( yes) - partition set-shift-points\nwith-approval-handling { - :check approval condition; - if (Is approved?) then (no) - :set approval_handler to - WAITING_APPROVAL; - :generate candidate path - with the new shifts; - else (\n yes) - :set new shift points to path_shifter; - note right - This process makes changes - to the final path shape. - end note - :register associated raw_shift_points; - note right - Used to keep the planning consistency - in the shift points generation. - end note - :clear WAITING_APPROVAL - in approval_handler; - endif - } -else (\n no) +start +partition plan() { +partition updatePathShifter() { +if (Is approved?) then (yes) +:add new shift lines to path shifter; +else (\n yes) +endif +} + +if (Is current status SUCCEEDED?) then (yes) +:removeRegisteredShiftLines(); +:output previous module output; +stop +else (no) +endif + +if (Is current status CANCEL?) then (yes) +:removeRegisteredShiftLines(); +:output previous module output; +stop +else (no) +endif + +if (Is in YIELD maneuver?) then (yes) +:removeRegisteredShiftLines(); +else (no) endif -partition path-generation { +:generate avoidance path; -:generateAvoidancePath; +:calculate turn signal; + +partition updateEgoBehavior() { +:insertPrepareVelocity(); note right - Final path is generated by path_shifter - from the calculated shift points with - Clothoid-Like curve primitives. + This module inserts slow down section so that the ego can avoid target object under lateral jerk constraints. +end note +:insertAvoidanceVelocity(); +note right + This module controls ego acceleration until the ego passes the side of the target object. end note -:generateExtendedDrivableArea; - -:modifyPathVelocityToPreventAccelerationOnAvoidance; +if (Is in YIELD maneuver?) then (yes) +:insertWaitPoint(); note right - Reduce acceleration during - avoidance to improve ride comfort. + This module inserts stop point. + The ego keeps stopping until this module judges the ego can avoid target object safely. end note +else (no) +endif -:resample path; +if (Is waiting operator approval?) then (yes) +:insertWaitPoint(); note right - To reduce path points for later node. + This module inserts stop point. + The ego keeps stopping until operator approves avoidance maneuver. end note +else (no) +endif + +:insertReturnDeadLine(); +} +:generate drivable area; } stop + @enduml ``` -## Overview of algorithm for target object filtering +## Target object filtering + +### Overview + +The module uses following conditions to filter avoidance target objects. + +| Check condition | Target class | Details | If conditions are not met | +| -------------------------------------------------------------------------------------------- | ------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ------------------------------- | +| Is an avoidance target class object? | All | Use can select avoidance target class from config file. | Never avoid it. | +| Is a stopped object? | All | Objects keep higher speed than `th_moving_speed` for longer period of time than `th_moving_time` is judged as moving. | Never avoid it. | +| Is within detection area? | All | The module creates detection area to filter target objects roughly based on lateral margin in config file. (see [here](#width-of-detection-area)) | Never avoid it. | +| Isn't there enough lateral distance between the object and path? | All | - | Never avoid it. | +| Is near the centerline of ego lane? | All | - | It depends on other conditions. | +| Is there a crosswalk near the object? | Pedestrian, Bicycle | The module don't avoid the Pedestrian and Bicycle nearer the crosswalk because the ego should stop in front of it if they're crossing road. (see [here](#for-crosswalk-users)) | Never avoid it. | +| Is the distance between the object and traffic light along the path longer than threshold? | Car, Truck, Bus, Trailer | The module used this condition if there is ambiguity as to whether the vehicle is a parked vehicle or not. | It depends on other conditions. | +| Is the distance between the object and crosswalk light along the path longer than threshold? | Car, Truck, Bus, Trailer | Same as above. | It depends on other conditions. | +| Is the stopping time longer than threshold? | Car, Truck, Bus, Trailer | Same as above. | It depends on other conditions. | +| Is within intersection? | Car, Truck, Bus, Trailer | The module assumes that there isn't any parked vehicle within intersection. | It depends on other conditions. | +| Is on ego lane? | Car, Truck, Bus, Trailer | - | It depends on other conditions. | +| Is a parked vehicle? | Car, Truck, Bus, Trailer | The module judges whether the vehicle is a parked vehicle based on its lateral offset. (see [here](#judge-if-its-a-parked-vehicle)) | It depends on other conditions. | +| Is merging to ego lane from other lane? | Car, Truck, Bus, Trailer | The module judges the vehicle behavior based on its yaw angle and offset direction. (see [here](#judge-vehicle-behavior)) | It depends on other conditions. | +| Is merging to other lane from ego lane? | Car, Truck, Bus, Trailer | Same as above. | It depends on other conditions. | + +### Common conditions + +#### Detection area + +The module generates detection area for target filtering based on following params: + +```yaml + # avoidance is performed for the object type with true + target_object: + ... + lateral_margin: + soft_margin: 0.3 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] + ... + # For target object filtering + target_filtering: + ... + # detection area generation parameters + detection_area: + static: false # [-] + min_forward_distance: 50.0 # [m] + max_forward_distance: 150.0 # [m] + backward_distance: 10.0 # [m] +``` + +##### Width of detection area + +1. get the largest lateral margin of all classes (Car, Truck, ...). The margin is sum of `soft_margin` and `hard_margin_for_parked_vehicle`. +2. the detection area width is sum of ego vehicle width and the largest lateral margin. + +##### Longitudinal distance of detection area + +If the parameter `detection_area.static` is set to `true`, the module creates detection area whose longitudinal distance is `max_forward_distance`. + +If the parameter `detection_area.static` is set to `false`, the module creates detection area so that the ego can avoid objects with minimum lateral jerk value. Thus, the longitudinal distance is depends on lateral maximum shift length, lateral jerk constraints and current ego speed. Additionally, it has to consider distance used for prepare phase. + +```c++ +... + 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(), speed); + + return std::clamp( + 1.5 * dynamic_distance + getNominalPrepareDistance(), + parameters_->object_check_min_forward_distance, + parameters_->object_check_max_forward_distance); +``` + +![fig](./images/target_filter/detection_area.svg) -### How to decide the target obstacles +![fig](./images/target_filter/detection_area_rviz.png) -The avoidance target should be limited to stationary objects (you should not avoid a vehicle waiting at a traffic light even if it blocks your path). Therefore, target vehicles for avoidance should meet the following specific conditions. +### Conditions for non-vehicle type objects -- It is in the vicinity of your lane (parametrized) -- It is stopped - - `threshold_speed_object_is_stopped`: parameter that be used for judge the object has stopped or not. - - `threshold_time_object_is_moving`: parameter that be used for chattering prevention. -- It is a specific class. - - User can limit avoidance targets. - - Fo now, avoidance module supports only vehicle. -- It is not being in the center of the route - - This means that the vehicle is parked on the edge of the lane. This prevents the vehicle from avoiding a vehicle waiting at a traffic light in the middle of the lane. However, this is not an appropriate implementation for the purpose. Even if a vehicle is in the center of the lane, it should be avoided if it has its hazard lights on, and this is a point that should be improved in the future as the recognition performance improves. -- Object is not behind ego(default: > -`2.0 m`) or too far(default: < `150.0 m`) and object is not behind the path goal. +#### For crosswalk users -![fig1](./images/target_vehicle_selection.svg) +If Pedestrian and Bicycle are closer to crosswalk than threshold 2.0m (hard coded for now), the module judges they're crossing the road and never avoids them. -### Parked-car detection +![fig](./images/target_filter/crosswalk.svg) -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`. +### Conditions for vehicle type objects + +#### Judge vehicle behavior + +The module classifies vehicles into following three behavior based on its yaw angle and offset direction. + +```yaml +# params for filtering objects that are in intersection +intersection: + yaw_deviation: 0.349 # [rad] (default 20.0deg) +``` + +| Behavior | Details | Figure | +| --------- | ---------------------------------------------------------------------------------------------------------------- | -------------------------------------------- | +| NONE | If the object's relative yaw angle to lane is less than threshold `yaw_deviation`, it is classified into `NONE`. | ![fig](./images/target_filter/none.png) | +| MERGING | See following flowchart. | ![fig](./images/target_filter/merging.png) | +| DEVIATING | See following flowchart. | ![fig](./images/target_filter/deviating.png) | + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title Judge vehicle behavior +start + +:calculate object relative yaw angle; +if(angle < threshold or angle > PI - threshold) then (yes) +:it is neither MERGING nor DEVIATING. (=NONE); +stop +else (no) +endif +if(Is the object on right side of ego path?) then (yes) +if(angle < 0.0 and -PI/2 < angle) then (yes) +#FF006C :DEVIATING; +stop +else (no) +if(angle > PI/2) then (yes) +#FF006C :DEVIATING; +stop +else (no) +endif +endif +else (no) +if(angle > 0.0 and PI/2 > angle) then (yes) +#FF006C :DEVIATING; +stop +else (no) +if(angle < -PI/2) then (yes) +#FF006C :DEVIATING; +stop +else (no) +endif +endif +endif +#00FFB1 :MERGING; +stop +@enduml + +``` + +#### Judge if it's a parked vehicle + +Not only the length from the centerline, but also the length from the road shoulder is calculated and used for the filtering process. In this logic, it calculates ratio of **actual shift length** to **shiftable shift length** as follow. If the result is larger than threshold `th_shiftable_ratio`, the module judges the vehicle as a parked vehicle. $$ -l_D = l_a - \frac{width}{2}, \\ -ratio = \frac{l_d}{l_D} +L_{d} = \frac{W_{lane} - W_{obj}}{2}, \\ +ratio = \frac{L_{a}}{L_{d}} $$ -- $l_d$ : actual shift length -- $l_D$ : shiftable length -- $l_a$ : distance between centerline and most left boundary. -- $width$ : object width +- $L_{d}$ : shiftable length. +- $L_{a}$ : actual shift length. +- $W_{lane}$ : lane width. +- $W_{obj}$ : 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. If the road has no road shoulders, it uses `object_check_min_road_shoulder_width` as a road shoulder width virtually. +![fig2](./images/target_filter/parked_vehicle.svg) -![fig2](./images/parked-car-detection.svg) +### Target object filtering -### Compensation for detection lost - -In order to prevent chattering of recognition results, once an obstacle is targeted, it is hold for a while even if it disappears. This is effective when recognition is unstable. However, since it will result in over-detection (increase a number of false-positive), it is necessary to adjust parameters according to the recognition accuracy (if `object_last_seen_threshold = 0.0`, the recognition result is 100% trusted). +| Situation | Details | Ego behavior | +| -------------------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------- | +| Vehicle is within intersection area defined in HDMap. The module ignores vehicle which is following lane or merging to ego lane. | ![fig](./images/target_filter/never_avoid_intersection.png) | Never avoid it. | +| Vehicle is on ego lane. There are adjacent lanes for both side. | ![fig](./images/target_filter/never_avoid_not_edge.png) | Never avoid it. | +| Vehicle is merging to other lane from ego lane. Most part of its footprint is on ego lane. | ![fig](./images/target_filter/never_avoid_deviating.png) | Never avoid it. | +| Vehicle is merging to ego lane from other lane. Most part of its footprint is on ego lane. | ![fig](./images/target_filter/never_avoid_merging.png) | Never avoid it. | +| Vehicle is not a obvious parked vehicle and stopping in front of the crosswalk or traffic light. | ![fig](./images/target_filter/never_avoid_stop_factor.png) | Never avoid it. | +| Vehicle stops on ego lane with pulling over to the side of the road. | ![fig](./images/target_filter/avoid_on_ego_lane.png) | Avoid it immediately. | +| Vehicle stops on adjacent lane. | ![fig](./images/target_filter/avoid_not_on_ego_lane.png) | Avoid it immediately. | +| Vehicle stops on ego lane without pulling over to the side of the road. | ![fig](./images/target_filter/ambiguous_parallel.png) | Set the parameter `avoidance_for_ambiguous_vehicle.enable` to `true`, the module avoids ambiguous vehicle. | +| Vehicle is merging to ego lane from other lane. | ![fig](./images/target_filter/ambiguous_merging.png) | Set the parameter `avoidance_for_ambiguous_vehicle.enable` to `true`, the module avoids ambiguous vehicle. | +| Vehicle is merging to other lane from ego lane. | ![fig](./images/target_filter/ambiguous_deviating.png) | Set the parameter `avoidance_for_ambiguous_vehicle.enable` to `true`, the module avoids ambiguous vehicle. | ### Flowchart +There are three main filtering functions `isSatisfiedWithCommonCondition()`, `isSatisfiedWithVehicleCondition()` and `isSatisfiedWithNonVehicleCondition()`. The filtering process is executed according to following flowchart. Additionally, the module checks avoidance necessity in `isNoNeedAvoidanceBehavior()` based on the object pose, ego path and lateral margin in config file. + ```plantuml @startuml skinparam defaultTextAlignment center skinparam noteTextAlignment left -title object filtering flowchart +title Object filtering flowchart start -if(object is satisfied with common target condition ?) then (yes) -if(vehicle can pass by the object without avoidance ?) then (yes) -:return false; +if(isSatisfiedWithCommonCondition()) then (yes) +if(isNoNeedAvoidanceBehavior()) then (yes) +#00FFB1 :IGNORE; stop else (\n no) endif else (\n no) -:return false; +#00FFB1 :IGNORE; stop endif - -if(object is vehicle type ?) then (yes) -if(object is satisfied with vehicle type target condition ?) then (yes) +if(Is vehicle type?) then (yes) +if(isSatisfiedWithVehicleCodition()) then (yes) else (\n no) -:return false; +#00FFB1 :IGNORE; stop endif else (\n no) -if(object is satisfied with non-vehicle type target condition ?) then (yes) +if(isSatisfiedWithNonVehicleCodition()) then (yes) else (\n no) -:return false; +#00FFB1 :IGNORE; stop endif endif - -#FF006C :return true; +#FF006C :AVOID; stop @enduml ``` +#### Common conditions + +At first, the function `isSatisfiedWithCommonCondition()` includes conditions used for all object class. + ```plantuml @startuml skinparam defaultTextAlignment center skinparam noteTextAlignment left -title filtering flow for all types object +title Common filtering flow start partition isSatisfiedWithCommonCondition() { -if(object is avoidance target type ?) then (yes) -if(object is moving more than threshold time ?) then (yes) -:return false; +if(Is object within detection area? (filtering roughly by position.)) then (yes) +if(Is object an avoidance target type?) then (yes) +if(Is moving object?) then (yes) +#00FFB1 :return false; stop else (\n no) -if(object is farther than forward distance threshold ?) then (yes) -:return false; +if(Is object farther than forward distance threshold ?) then (yes) +#00FFB1 :return false; stop else (\n no) -If(object is farther than backward distance threshold ?) then (yes) -:return false; +If(Is object farther than backward distance threshold ?) then (yes) +#00FFB1 :return false; stop else (\n no) endif endif endif else (\n no) -:return false; +#00FFB1 :return false; +stop +endif +else (\n no) +#00FFB1 :return false; stop endif #FF006C :return true; @@ -283,14 +462,26 @@ stop } @enduml + ``` +#### Conditions for vehicle + +Target class: + +- Car +- Truck +- Bus +- Trailer + +As a next step, object is filtered by condition specialized for its class. + ```plantuml @startuml skinparam defaultTextAlignment center skinparam noteTextAlignment left -title filtering flow for vehicle type objects +title Filtering flow for vehicle type objects start partition isSatisfiedWithVehicleCodition() { @@ -298,34 +489,127 @@ if(object is force avoidance target ?) then (yes) #FF006C :return true; stop else (\n no) -if(object is nearer lane centerline than threshold ?) then (yes) -:return false; + +if(isNeverAvoidanceTarget()) then (yes) +#00FFB1 :return false; stop else (\n no) -if(object is on same lane for ego ?) then (yes) -if(object is shifting right or left side road shoulder more than threshold ?) then (yes) + +if(isObviousAvoidanceTarget()) then (yes) #FF006C :return true; stop else (\n no) -:return false; + +if(Is stopping time longer than threshold?) then (no) +#00FFB1 :return false; stop +else (\n yes) + +partition filtering ambiguous vehicle { +if(Is object within intersection?) then (yes) +if(Is object deviating from ego lane?) then (yes) +#FF006C :return true(ambiguous); +stop +else (\n no) endif else (\n no) -if(object is in intersection ?) then (no) -#FF006C :return true; + +if(Is object merging to ego lane?) then (yes) +#FF006C :return true(ambiguous); stop -else (\n yes) -if(object pose is paralell to ego lane ?) then (no) +else (\n no) +endif + +if(Is object deviating from ego lane?) then (yes) +#FF006C :return true(ambiguous); +stop +else (\n no) +endif + +if(Is object parallel to ego lane?) then (yes) #FF006C :return true; stop -else (\n yes) -:return false; +else (\n no) +endif + +endif +} +endif +endif +endif +endif +#00FFB1 :return false; +stop +} + +@enduml + +``` + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title Filter vehicle which is obviously not an avoidance target +start + +partition isNeverAvoidanceTarget() { +if(Is object within intersection?) then (yes) + +if(Is object parallel to ego lane?) then (yes) +#00FFB1 :return true; +stop +else (\n no) +endif + +if(Is object merging to ego lane?) then (yes) +#00FFB1 :return true; +stop +else (\n no) +endif + +else (\n no) +endif + +if(Is object merging to ego lane?) then (yes) +if(Is overhang distance larger than threshold?) then (yes) +#00FFB1 :return true; stop +else (\n no) +endif +else (\n no) endif + +if(Is object deviating from ego lane?) then (yes) +if(Is overhang distance larger than threshold?) then (yes) +#00FFB1 :return true; +stop +else (\n no) endif +else (\n no) endif + +if(Is object on ego lane?) then (yes) +if(Is object on edge lane?) then (no) +#00FFB1 :return true; +stop +else (\n no) endif +else (\n no) endif + +if(isCloseToStopFactor()) then (yes) +if(Is object on ego lane? AND Isn't object a parked vehile?) then (no) +#00FFB1 :return true; +stop +else (\n no) +endif +else (\n no) +endif + +#FF006C :return false; +stop } @enduml @@ -336,12 +620,48 @@ endif skinparam defaultTextAlignment center skinparam noteTextAlignment left -title filtering flow for non-vehicle type objects +title Filter vehicle which is obviously an avoidance target +start + +partition isObviousAvoidanceTarget() { +if(Is object within intersection?) then (yes) +else (\n no) +if(Is object parallel to ego lane? AND Is object a parked vehicle?) then (yes) +#FF006C :return true; +stop +else (\n no) +endif + +if(Is object parallel to ego lane? AND Isn't object on ego lane?) then (yes) +#FF006C :return true; +stop +else (\n no) +endif +endif + +#00FFB1 :return false; +stop +} + +@enduml +``` + +#### Conditions for non-vehicle objects + +- Pedestrian +- Bicycle + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title Filtering flow for non-vehicle type objects start partition isSatisfiedWithNonVehicleCodition() { -if(object is nearer crosswalk than threshold ?) then (yes) -:return false; +if(isWithinCrosswalk()) then (yes) +#00FFB1 :return false; stop else (\n no) endif @@ -352,90 +672,320 @@ stop @enduml ``` -## Overview of algorithm for avoidance path generation +## When target object has gone + +User can select the ego behavior when the target object has gone. + +```yaml +cancel: + enable: true # [-] +``` + +If above parameter is `true`, this module reverts avoidance path when following conditions are met. + +- all target objects have gone. +- the ego vehicle hasn't initiated avoidance maneuver yet. + +![fig](./images/cancel/cancel.png) + +If the parameter is `false`, this module keeps running even after target object has gone. + +## Path generation ### How to prevent shift line chattering that is caused by perception noise -Since object recognition results contain noise related to position ,orientation and boundary size, if the raw object recognition results are used in path generation, the avoidance path will be directly affected by the noise. +Since object recognition result contains noise related to position, orientation and polygon shape, if the module uses the raw object recognition results in path generation, the output path will be directly affected by the noise. Therefore, in order to reduce the influence of the noise, this module generates polygon for each target objects, and the output path is generated based on that. + +![fig](./images/path_generation/envelope_polygon_rviz.png) + +The envelope polygon is a rectangle box, whose size depends on object's polygon and buffer parameter `envelope_buffer_margin`. Additionally, it is always parallel to reference path. When the module finds target object for the first time, it initializes the polygon. + +```yaml + car: + ... + envelope_buffer_margin: 0.5 # [m] FOR DEVELOPER +``` + +![fig](./images/path_generation/envelope_polygon.png) + +The module creates one-shot envelope polygon by using latest object pose and raw polygon in every planning cycle. On the other hand, the module envelope polygon information which is created in last planning cycle as well in order to update envelope polygon according to following logic. If the one-shot envelope polygon is not within previous envelope polygon, the module creates new envelope polygon. Otherwise, it keeps previous envelope polygon. By doing this process, the envelope polygon size and pose will converge even if perception output includes noise in object pose or shape. + +![fig](./images/path_generation/polygon_update.png) + +### Relationship between envelope polygon and avoidance path + +The avoidance path has two shift section, whose start or end point position depends on envelope polygon. The end point of avoidance shift section and start point of return shift section are fixed based on envelope polygon and the other side edges are dynamically changed based on ego speed, shift length, lateral jerk constraints, etc... + +The lateral positions of the two points are decided so that there can be enough space (=lateral margin) between ego body and the most overhang point of envelope polygon edge points. User can adjust lateral margin with following parameters. -Therefore, in order to reduce the influence of the noise, avoidance module generate a envelope polygon for the avoidance target that covers it, and the avoidance path should be generated based on that polygon. The envelope polygons are generated so that they are parallel to the reference path and the polygon size is larger than the avoidance target (define by `object_envelope_buffer`). The position and size of the polygon is not updated as long as the avoidance target exists within that polygon. +```yaml + car: + ... + lateral_margin: + soft_margin: 0.3 # [m] + hard_margin: 0.2 # [m] + hard_margin_for_parked_vehicle: 0.7 # [m] +``` + +The longitudinal positions depends on envelope polygon, ego vehicle specification and following parameters. The longitudinal distance between avoidance shift section end point and envelope polygon (=front longitudinal buffer) is sum of `front_overhang` defined in `vehicle_info.param.yaml` and `longitudinal_margin` if the parameter `consider_front_overhang` is `true`. If `consider_front_overhang` is `false`, only `longitudinal_margin` is considered. Similarly, the distance between return shift section start point and envelope polygon (=rear longitudinal buffer) is sum of `rear_overhang` and `longitudinal_margin`. ```yaml -# default value -object_envelope_buffer: 0.3 # [m] + + target_object: + car: + ... + longitudinal_margin: 0.0 # [m] + + ... + avoidance: + ... + longitudinal: + ... + consider_front_overhang: true # [-] + consider_rear_overhang: true # [-] ``` -![fig1](./images/envelope_polygon.svg) +![fig](./images/path_generation/margin.png) -![fig1](./images/shift_line_generation.svg) +### Shift length calculation -### Computing Shift Length and Shift Points +The lateral shift length is sum of `overhang_distance`, lateral margin, whose value is set in config file, and the half of ego vehicle width defined in `vehicle_info.param.yaml`. On the other hand, the module limits the shift length depending on the space which the module can use for avoidance maneuver and the parameters `soft_drivable_bound_margin` `hard_drivable_bound_margin`. Basically, the shift length is limited so that the ego doesn't get closer than `soft_drivable_bound_margin` to drivable boundary. But it allows to relax the threshold `soft_drivable_bound_margin` to `hard_drivable_bound_margin` when the road is narrow. -The lateral shift length is affected by 4 variables, namely `lateral_collision_safety_buffer`, `lateral_collision_margin`, `vehicle_width` and `overhang_distance`. The equation is as follows +![fig](./images/path_generation/lateral.png) + +Usable lane for avoidance module can be selected by config file. + +```yaml + ... + use_adjacent_lane: true + use_opposite_lane: true +``` -```C++ -avoid_margin = lateral_collision_margin + lateral_collision_safety_buffer + 0.5 * vehicle_width -max_allowable_lateral_distance = to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width -if(isOnRight(o)) +When user set parameter both `use_adjacent_lane` and `use_opposite_lane` to `true`, it is possible to use opposite lane. + +![fig](./images/path_generation/opposite_direction.png) + +When user only set parameter `use_adjacent_lane` to `true`, the module doesn't create path that overlaps opposite lane. + +![fig](./images/path_generation/same_direction.png) + +### Shift line generation + +As mentioned above, the end point of avoidance shift path and the start point of return shift path, which are FIXED points, are calculated from envelope polygon. As a next step, the module adjusts the other side points depending on shift length, current ego speed and lateral jerk constrain params defined in config file. + +Since the two points are always on centerline of ego lane, the module only calculates longitudinal distance between shift start and end point based on following function. This function is defined in path shifter library, so please see [this](../autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md) page as well. + +```c++ +double PathShifter::calcLongitudinalDistFromJerk( + const double lateral, const double jerk, const double velocity) { - shift_length = avoid_margin + overhang_distance + const double j = std::abs(jerk); + const double l = std::abs(lateral); + const double v = std::abs(velocity); + if (j < 1.0e-8) { + return 1.0e10; + } + return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v; } -else +``` + +We call the line which connects shift start and end point `shift_line`, whom the avoidance path is generated from with spline completion. + +The start point of avoidance has another longitudinal constraint. In order to keep turning on the blinker for few seconds before starting avoidance maneuver, the avoidance start point must be further than the value (we call the distance `prepare_length`.) depending on ego speed from ego position. + +```yaml +longitudinal: + min_prepare_time: 1.0 # [s] + max_prepare_time: 2.0 # [s] + min_prepare_distance: 1.0 # [m] +``` + +The `prepare_length` is calculated as the product of ego speed and `max_prepare_time`. (When the ego speed is zero, `min_prepare_distance` is used.) + +![fig](./images/path_generation/shift_line.png) + +## Safety check + +This feature can be enable by setting following parameter to `true`. + +```yaml + safety_check: + ... + enable: true # [-] +``` + +This module pay attention not only avoidance target objects but also non-target objects that are near the avoidance path, and if avoidance path is unsafe due to surround objects, it reverts avoidance maneuver and yields lane to them. + +![fig](./images/safety_check/safety_check_flow.png) + +### Yield maneuver + +Additionally, this module basically inserts stop point in front of avoidance target during yielding maneuver in order to keep enough distance to avoid the target after it is safe situation. If the shift side lane is congested, the ego stops the point and waits. + +This feature can be enable by setting following parameter to `true`. + +```yaml +yield: + enable: true # [-] +``` + +![fig](./images/safety_check/stop.png) + +But if the lateral margin is larger than `hard_margin` (or `hard_margin_for_parked_vehicle`), this module doesn't insert stop point because the ego can pass side of the object safely without avoidance maneuver. + +![fig](./images/safety_check/not_stop.png) + +### Safety check target lane + +User can select safety check area by following params. Basically, we recommend following configuration to check only sift side lane. But if user want to confirm safety strictly, please set `check_current_lane` and/or `check_other_side_lane` to `true`. + +```yaml + safety_check: + ... + check_current_lane: false # [-] + check_shift_side_lane: true # [-] + check_other_side_lane: false # [-] +``` + +In avoidance module, the function `path_safety_checker::isCentroidWithinLanelet` is used for filtering objects by lane. + +```c++ +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - shift_length = avoid_margin - overhang_distance + 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()); } ``` -The following figure illustrates these variables(This figure just shows the max value of lateral shift length). +!!! info + + If it set `check_current_lane` and/or `check_other_side_lane` to `true`, maybe the possibility of false positive occurring and unnecessary yield maneuver execution increases. + +### Safety check algorithm + +This module uses common safety check logic implemented in `path_safety_checker` library. See [this](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) page. + +### Limitation + +#### Limitation-1 + +The current behavior when the module judges it's unsafe is so conservative because it is difficult to achieve aggressive (e.g. increase speed in order to increase the distance to behind vehicle.) for current planning architecture. + +#### Limitation-2 + +The yield maneuver is executed **ONLY** when the vehicle has **NOT** initiated avoidance maneuver yet. (This module checks objects on opposite lane but it's necessary to find very far objects to judge unsafe before avoidance maneuver.) If it detects vehicle which is closing to ego during avoidance maneuver, this module doesn't neither revert the path nor insert stop point. For now, there is no feature to deal with this situation in this module. Thus, new module is needed to adjust path to avoid moving objects, or operator should do override. + +!!! info + + This module has a threshold parameter `th_avoid_execution` for the shift length, and judge that the vehicle is initiating avoidance if the vehicle current shift exceeds the value. + +## Other features + +### Compensation for detection lost + +In order to prevent chattering of recognition results, once an obstacle is targeted, it is hold for a while even if it disappears. This is effective when recognition is unstable. However, since it will result in over-detection (increase a number of false-positive), it is necessary to adjust parameters according to the recognition accuracy (if `object_last_seen_threshold = 0.0`, the recognition result is 100% trusted). + +### Drivable area expansion + +This module supports drivable area expansion for following polygons defined in HDMap. + +- intersection area +- hatched road marking +- freespace area + +Please set the flags to `true` when user wants to make it possible to use those areas in avoidance maneuver. + +```yaml +# drivable area setting +use_adjacent_lane: true +use_opposite_lane: true +use_intersection_areas: true +use_hatched_road_markings: true +use_freespace_areas: true +``` + +| | | | +| --------------------- | ---------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| adjacent lane | ![fig](./images/advanced/avoidance_same_direction.png) | | +| opposite lane | ![fig](./images/advanced/avoidance_opposite_direction.png) | | +| intersection area | ![fig](./images/advanced/avoidance_intersection.png) | The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) | +| hatched road markings | ![fig](./images/advanced/avoidance_zebra.png) | The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) | +| freespace area | ![fig](./images/advanced/avoidance_freespace.png) | The freespace area is defined on Lanelet map. (unstable) | -![shift_point_and_its_constraints](./images/avoidance_module-shift_point_and_its_constraints.drawio.png) +## Future extensions / Unimplemented parts -### Rationale of having safety buffer and safety margin +- **Planning on the intersection** + - If it is known that the ego vehicle is going to stop in the middle of avoidance execution (for example, at a red traffic light), sometimes the avoidance should not be executed until the vehicle is ready to move. This is because it is impossible to predict how the environment will change during the stop. This is especially important at intersections. -To compute the shift length, additional parameters that can be tune are `lateral_collision_safety_buffer` and `road_shoulder_safety_margin`. +![fig](./images/intersection_problem.drawio.svg) -- The `lateral_collision_safety_buffer` parameter is used to set a safety gap that will act as the final line of defense when computing avoidance path. - - The rationale behind having this parameter is that the parameter `lateral_collision_margin` might be changing according to the situation for various reasons. Therefore, `lateral_collision_safety_buffer` will act as the final line of defense in case of the usage of `lateral_collision_margin` fails. - - It is recommended to set the value to more than half of the ego vehicle's width. -- The `road_shoulder_safety_margin` will prevent the module from generating a path that might cause the vehicle to go too near the road shoulder or adjacent lane dividing line. +- **Safety Check** -![shift_length_parameters](./images/shift_length_parameters.drawio.svg) + - In the current implementation, it is only the jerk limit that permits the avoidance execution. It is needed to consider the collision with other vehicles when change the path shape. -### Generating path only within lanelet boundaries +- **Consideration of the speed of the avoidance target** -The shift length is set as a constant value before the feature is implemented. Setting the shift length like this will cause the module to generate an avoidance path regardless of actual environmental properties. For example, the path might exceed the actual road boundary or go towards a wall. Therefore, to address this limitation, in addition to [how to decide the target obstacle](#how-to-decide-the-target-obstacles), the module also takes into account the following additional element + - In the current implementation, only stopped vehicle is targeted as an avoidance target. It is needed to support the overtaking function for low-speed vehicles, such as a bicycle. (It is actually possible to overtake the low-speed objects by changing the parameter, but the logic is not supported and thus the safety cannot be guaranteed.) + - The overtaking (e.g., to overtake a vehicle running in front at 20 km/h at 40 km/h) may need to be handled outside the avoidance module. It should be discussed which module should handle it. -- The obstacles' current lane and position. -- The road shoulder with reference to the direction to avoid. +- **Cancel avoidance when target disappears** + + - In the current implementation, even if the avoidance target disappears, the avoidance path will remain. If there is no longer a need to avoid, it must be canceled. + +- **Improved performance of avoidance target selection** -These elements are used to compute the distance from the object to the road's shoulder (`to_road_shoulder_distance`). The parameters `use_adjacent_lane` and `use_opposite_lane` allows further configuration of the to `to_road_shoulder_distance`. The following image illustrates the configuration. + - Essentially, avoidance targets are judged based on whether they are static objects or not. For example, a vehicle waiting at a traffic light should not be avoided because we know that it will start moving in the future. However this decision cannot be made in the current Autoware due to the lack of the perception functions. Therefore, the current avoidance module limits the avoidance target to vehicles parked on the shoulder of the road, and executes avoidance only for vehicles that are stopped away from the center of the lane. However, this logic cannot avoid a vehicle that has broken down and is stopped in the center of the lane, which should be recognized as a static object by the perception module. There is room for improvement in the performance of this decision. -![obstacle_to_road_shoulder_distance](./images/obstacle_to_road_shoulder_distance.drawio.svg) +- **Resampling path** + - Now the rough resolution resampling is processed to the output path in order to reduce the computational cost for the later modules. This resolution is set to a uniformly large value (e.g. `5m`), but small resolution should be applied for complex paths. + +## Debug + +### Show `RCLCPP_DEBUG` on console + +All of debug messages are logged in following namespaces. + +- `planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.static_obstacle_avoidance` or, +- `planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.static_obstacle_avoidance.utils` + +User can see debug information by following command. + +```bash +ros2 service call /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/config_logger logging_demo/srv/ConfigLogger "{logger_name: 'planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.static_obstacle_avoidance', level: DEBUG}" +``` -If one of the following conditions is `false`, then the shift point will not be generated. +### Visualize debug markers -- The distance to shoulder of road is enough +Use can enable to publish debug markers by following parameters. -```C++ -avoid_margin = lateral_collision_margin + lateral_collision_safety_buffer + 0.5 * vehicle_width -avoid_margin <= (to_road_shoulder_distance - 0.5 * vehicle_width - road_shoulder_safety_margin) +```yaml +debug: + enable_other_objects_marker: false + enable_other_objects_info: false + enable_detection_area_marker: false + enable_drivable_bound_marker: false + enable_safety_check_marker: false + enable_shift_line_marker: false + enable_lane_marker: false + enable_misc_marker: false ``` -- The obstacle intrudes into the current driving path. +![fig](./images/debug/debug_marker_rviz.png) - - when the object is on right of the path +### Echoing debug message to find out why the objects were ignored - ```C++ - -overhang_dist<(lateral_collision_margin + lateral_collision_safety_buffer + 0.5 * vehicle_width) - ``` +If for some reason, no shift point is generated for your object, you can check for the failure reason via `ros2 topic echo`. - - when the object is on left of the path +![avoidance_debug_message_array](./images/avoidance_debug_message_array.png) - ```C++ - overhang_dist<(lateral_collision_margin + lateral_collision_safety_buffer + 0.5 * vehicle_width) - ``` +To print the debug message, just run the following -## Details of algorithm for avoidance path generation +```bash +ros2 topic echo /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance_debug_message_array +``` + +## Appendix: Shift line generation pipeline ### Flow-chart of the process @@ -565,7 +1115,7 @@ To solve that, this module provides a parameter for the minimum avoidance speed, - If the ego vehicle speed is lower than "nominal" minimum speed, use the minimum speed in the calculation of the jerk. - If the ego vehicle speed is lower than "sharp" minimum speed and a nominal lateral jerk is not enough for avoidance (the case where the ego vehicle is stopped close to the obstacle), use the "sharp" minimum speed in the calculation of the jerk (it should be lower than "nominal" speed). -![fig1](./images/how_to_decide_path_shape_one_object.drawio.svg) +![fig](./images/how_to_decide_path_shape_one_object.drawio.svg) #### Multiple obstacle case (one direction) @@ -575,13 +1125,13 @@ Generate shift points for multiple obstacles. All of them are merged to generate For the details of the shift point filtering, see [filtering for shift points](#filtering-for-shift-points). -![fig1](./images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg) +![fig](./images/how_to_decide_path_shape_multi_object_one_direction.drawio.svg) #### Multiple obstacle case (both direction) Generate shift points for multiple obstacles. All of them are merged to generate new shift points. If there are areas where the desired shifts conflict in different directions, the sum of the maximum shift amounts of these areas is used as the final shift amount. The rest of the process is the same as in the case of one direction. -![fig1](./images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg) +![fig](./images/how_to_decide_path_shape_multi_object_both_direction.drawio.svg) #### Filtering for shift points @@ -592,221 +1142,8 @@ The shift points are modified by a filtering process in order to get the expecte - Similar gradient removal: Connect two shift points with a straight line, and remove the shift points in between if their shift amount is in the vicinity of the straight line. - Remove momentary returns: For shift points that reduce the avoidance width (for going back to the center line), if there is enough long distance in the longitudinal direction, remove them. -## Other features - -### Drivable area expansion - -This module has following parameters that sets which areas the path may extend into when generating an avoidance path. - -```yaml -# drivable area setting -use_adjacent_lane: true -use_opposite_lane: true -use_intersection_areas: false -use_hatched_road_markings: false -``` - -#### adjacent lane - -![fig1](./images/use_adjacent_lane.svg) - -#### opposite lane - -![fig1](./images/use_opposite_lane.svg) - -#### intersection areas - -The intersection area is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) - -![fig1](./images/use_intersection_areas.svg) - -#### hatched road markings - -The hatched road marking is defined on Lanelet map. See [here](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md#hatched-road-markings-area) - -![fig1](./images/use_hatched_road_markings.svg) - -### Safety check - -The avoidance module has a safety check logic. The result of safe check is used for yield maneuver. It is enable by setting `enable` as `true`. - -```yaml -# safety check configuration -enable: true # [-] -check_current_lane: false # [-] -check_shift_side_lane: true # [-] -check_other_side_lane: false # [-] -check_unavoidable_object: false # [-] -check_other_object: true # [-] - -# collision check parameters -check_all_predicted_path: false # [-] -time_horizon: 10.0 # [s] -idling_time: 1.5 # [s] -safety_check_backward_distance: 50.0 # [m] -safety_check_accel_for_rss: 2.5 # [m/ss] -``` - -![fig1](./images/safety_check_flowchart.svg) - -`safety_check_backward_distance` is the parameter related to the safety check area. The module checks a collision risk for all vehicle that is within shift side lane and between object `object_check_forward_distance` ahead and `safety_check_backward_distance` behind. - -![fig1](./images/safety_check_step_1.svg) - -**NOTE**: Even if a part of an object polygon overlaps the detection area, if the center of gravity of the object does not exist on the lane, the vehicle is excluded from the safety check target. - ---- - -Judge the risk of collision based on ego future position and object prediction path. The module calculates Ego's future position in the time horizon (`safety_check_time_horizon`), and use object's prediction path as object future position. - -![fig1](./images/safety_check_step_2.svg) - -After calculating the future position of Ego and object, the module calculates the lateral/longitudinal deviation of Ego and the object. The module also calculates the lateral/longitudinal margin necessary to determine that it is safe to execute avoidance maneuver, and if both the lateral and longitudinal distances are less than the margins, it determines that there is a risk of a collision at that time. - -![fig1](./images/safety_check_margin.svg) - -The value of the longitudinal margin is calculated based on Responsibility-Sensitive Safety theory ([RSS](https://newsroom.intel.com/articles/rss-explained-five-rules-autonomous-vehicle-safety/#gs.ljzofv)). The `safety_check_idling_time` represents $T_{idle}$, and `safety_check_accel_for_rss` represents $a_{max}$. - -$$ -D_{lon} = V_{ego}T_{idle} + \frac{1}{2}a_{max}T_{idle}^2 + \frac{(V_{ego} + a_{max}T_{idle})^2}{2a_{max}} - \frac{V_{obj}^2}{2a_{max}} -$$ - -The lateral margin is changeable based on ego longitudinal velocity. If the vehicle is driving at a high speed, the lateral margin should be larger, and if the vehicle is driving at a low speed, the value of the lateral margin should be set to a smaller value. Thus, the lateral margin for each vehicle speed is set as a parameter, and the module determines the lateral margin from the current vehicle speed as shown in the following figure. - -![fig1](./images/dynamic_lateral_margin.svg) - -```yaml -target_velocity_matrix: - col_size: 5 - matrix: [2.78 5.56 ... 16.7 # target velocity [m/s] - 0.50 0.75 ... 1.50] # margin [m] -``` - ---- - -### Yield maneuver - -#### Overview - -If an avoidance path can be generated and it is determined that avoidance maneuver should not be executed due to surrounding traffic conditions, the module executes YIELD maneuver. In yield maneuver, the vehicle slows down to the target vehicle velocity (`yield_velocity`) and keep that speed until the module judge that avoidance path is safe. If the YIELD condition goes on and the vehicle approaches the avoidance target, it stops at the avoidable position and waits until the safety is confirmed. - -```yaml -# For yield maneuver -yield_velocity: 2.78 # [m/s] -``` - -![fig1](./images/yield_maneuver.svg) - -**NOTE**: In yield maneuver, the vehicle decelerates target velocity under constraints. - -```yaml -nominal_deceleration: -1.0 # [m/ss] -nominal_jerk: 0.5 # [m/sss] -``` - -If it satisfies following all of three conditions, the module inserts stop point in front of the avoidance target with an avoidable interval. - -- Can't pass through the side of target object without avoidance. -- There is enough lane width to avoid target object. -- In waiting approval or yield maneuver. - -The module determines that it is NOT passable without avoidance if the object overhang is less than the threshold. - -```yaml -lateral_passable_collision_margin: 0.5 # [-] -``` - -$$ -L_{overhang} < \frac{W}{2} + L_{margin} (not passable) -$$ - -The $W$ represents vehicle width, and $L_{margin}$ represents `lateral_passable_collision_margin`. - -#### Limitation - -##### Limitation1 - -The current behavior in unsafe condition is just slow down and it is so conservative. It is difficult to achieve aggressive behavior in the current architecture because of modularity. There are many modules in autoware that change the vehicle speed, and the avoidance module cannot know what speed planning they will output, so it is forced to choose a behavior that is as independent of other modules' processing as possible. - -##### Limitation2 - -The YIELD maneuver is executed **ONLY** when the vehicle has **NOT** initiated avoidance maneuver. The module has a threshold parameter (`avoidance_initiate_threshold`) for the amount of shifting and determines that the vehicle is initiating avoidance if the vehicle current shift exceeds the threshold. - -$$ -SHIFT_{current} > L_{threshold} -$$ - -![fig1](./images/yield_limitation.svg) - -![fig1](./images/yield_maneuver_flowchart.svg) - ---- - -### Avoidance cancelling maneuver - -If `enable_cancel_maneuver` parameter is true, Avoidance Module takes different actions according to the situations as follows: - -- If vehicle stops: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is stopping, the avoidance path will cancelled. -- If vehicle is in motion, but avoidance maneuver doesn't started: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is not started avoidance maneuver, the avoidance path will cancelled. -- If vehicle is in motion, avoidance maneuver started: If there is any object in the path of the vehicle, the avoidance path is generated,but if this object goes away while the vehicle is started avoidance maneuver, the avoidance path will not cancelled. - -If `enable_cancel_maneuver` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. - -## How to keep the consistency of the optimize-base path generation logic - -WIP - -## Parameters +## Appendix: All parameters The avoidance specific parameter configuration file can be located at `src/autoware/launcher/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml`. {{ json_to_markdown("planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json") }} - -## Future extensions / Unimplemented parts - -- **Planning on the intersection** - - If it is known that the ego vehicle is going to stop in the middle of avoidance execution (for example, at a red traffic light), sometimes the avoidance should not be executed until the vehicle is ready to move. This is because it is impossible to predict how the environment will change during the stop. This is especially important at intersections. - -![fig1](./images/intersection_problem.drawio.svg) - -- **Safety Check** - - - In the current implementation, it is only the jerk limit that permits the avoidance execution. It is needed to consider the collision with other vehicles when change the path shape. - -- **Consideration of the speed of the avoidance target** - - - In the current implementation, only stopped vehicle is targeted as an avoidance target. It is needed to support the overtaking function for low-speed vehicles, such as a bicycle. (It is actually possible to overtake the low-speed objects by changing the parameter, but the logic is not supported and thus the safety cannot be guaranteed.) - - The overtaking (e.g., to overtake a vehicle running in front at 20 km/h at 40 km/h) may need to be handled outside the avoidance module. It should be discussed which module should handle it. - -- **Cancel avoidance when target disappears** - - - In the current implementation, even if the avoidance target disappears, the avoidance path will remain. If there is no longer a need to avoid, it must be canceled. - -- **Improved performance of avoidance target selection** - - - Essentially, avoidance targets are judged based on whether they are static objects or not. For example, a vehicle waiting at a traffic light should not be avoided because we know that it will start moving in the future. However this decision cannot be made in the current Autoware due to the lack of the perception functions. Therefore, the current avoidance module limits the avoidance target to vehicles parked on the shoulder of the road, and executes avoidance only for vehicles that are stopped away from the center of the lane. However, this logic cannot avoid a vehicle that has broken down and is stopped in the center of the lane, which should be recognized as a static object by the perception module. There is room for improvement in the performance of this decision. - -- **Resampling path** - - Now the rough resolution resampling is processed to the output path in order to reduce the computational cost for the later modules. This resolution is set to a uniformly large value (e.g. `5m`), but small resolution should be applied for complex paths. - -## How to debug - -### Publishing Visualization Marker - -Developers can see what is going on in each process by visualizing all the avoidance planning process outputs. The example includes target vehicles, shift points for each object, shift points after each filtering process, etc. - -![fig1](./images/avoidance-debug-marker.png) - -To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner avoidance.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `static_obstacle_avoidance.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance` in `rviz2`. - -### Echoing debug message to find out why the objects were ignored - -If for some reason, no shift point is generated for your object, you can check for the failure reason via `ros2 topic echo`. - -![avoidance_debug_message_array](./images/avoidance_debug_message_array.png) - -To print the debug message, just run the following - -```bash -ros2 topic echo /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance_debug_message_array -``` diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_freespace.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_freespace.png new file mode 100644 index 0000000000000..f6f0cdf72f495 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_freespace.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_intersection.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_intersection.png new file mode 100644 index 0000000000000..13a06f01ac6de Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_intersection.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_opposite_direction.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_opposite_direction.png new file mode 100644 index 0000000000000..0f17ea914abed Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_opposite_direction.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_same_direction.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_same_direction.png new file mode 100644 index 0000000000000..a2247d8cda185 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_same_direction.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_zebra.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_zebra.png new file mode 100644 index 0000000000000..b129ddeacff77 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/advanced/avoidance_zebra.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance-debug-marker.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance-debug-marker.png deleted file mode 100644 index 70947c5aa086b..0000000000000 Binary files a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance-debug-marker.png and /dev/null differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig1.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig.drawio.svg similarity index 100% rename from planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig1.drawio.svg rename to planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_design.fig.drawio.svg diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png deleted file mode 100644 index fbeb3629a34a6..0000000000000 Binary files a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/avoidance_module-shift_point_and_its_constraints.drawio.png and /dev/null differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/cancel/cancel.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/cancel/cancel.png new file mode 100644 index 0000000000000..1d10fddf84d30 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/cancel/cancel.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/debug/debug_marker_rviz.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/debug/debug_marker_rviz.png new file mode 100644 index 0000000000000..aee050a3388f9 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/debug/debug_marker_rviz.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/dynamic_lateral_margin.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/dynamic_lateral_margin.svg deleted file mode 100644 index 527ed9395b495..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/dynamic_lateral_margin.svg +++ /dev/null @@ -1,144 +0,0 @@ - - - - - - - - - - - - - - - - - -
-
-
- velocity [m/s] -
-
-
- - velocity [m/s] - - - - - - -
-
-
- lateral margin [m] -
-
-
- - lateral margin [m] - - - - - - - - - - - - - - - - -
-
-
- define representative points in config file -
-
-
- - define representati... - - - - - - - -
-
-
- linear interpolation between two representative points -
-
-
- - linear interpolation between two... - - - - - - -
-
-
- Ve -
-
-
- - Ve - - - - - - -
-
-
- Dlat -
-
-
- - Dlat - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/envelope_polygon.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/envelope_polygon.svg deleted file mode 100644 index d997551399987..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/envelope_polygon.svg +++ /dev/null @@ -1,272 +0,0 @@ - - - - - - - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - - - -
-
-
- Step:T -
-
-
- - Step:T - - - - - - -
-
-
- Step:T+1 (before update) -
-
-
- - Step:T+1 (befor... - - - - - - -
-
-
- Step:T+1 (after update) -
-
-
- - Step:T+1 (aft... - - - - - - -
-
-
- create new Envelope Polygon -
-
-
- - create new Envelope Polygon - - - - - - - -
-
-
- avoidance target is not within Envelope Polygon -
-
-
- - avoidance target is not with... - - - - - - -
-
-
- avoidance target is within Envelope Polygon -
-
-
- - avoidance target is within E... - - - - - - -
-
-
- keep current Envelope Polygon -
-
-
- - keep current Envelope Polygon - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg deleted file mode 100644 index ae024ae99da47..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/obstacle_to_road_shoulder_distance.drawio.svg +++ /dev/null @@ -1,235 +0,0 @@ - - - - - - - - - - - - - - -
-
-
- - - enable_avoidance_over_same_direction == false -
- - -
-
-
- - enable_avoidance_over_same_direction == false - - - - - - - - - -
-
-
- - to_road_shoulder_distance - -
-
-
- - to_road_shoulder_distance - - - - - - - - - - - - - - - - - - -
-
-
- - - enable_avoidance_over_same_direction == true && enable_avoidance_over_opposite_direction == false -
- - -
-
-
- - enable_avoidance_over_same_direction == true && enable_a... - - - - - - - - - -
-
-
- - to_road_shoulder_distance - -
-
-
- - to_road_shoulder_distance - - - - - - - - - - - - - - - - - - -
-
-
- - - enable_avoidance_over_same_direction == true && enable_avoidance_over_opposite_direction == true -
- - -
-
-
- - enable_avoidance_over_same_direction == true && enable_a... - - - - - - - - - -
-
-
- - to_road_shoulder_distance - -
-
-
- - to_road_shoulder_distance - - - - - - - - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/parked-car-detection.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/parked-car-detection.svg deleted file mode 100644 index 79787f1f60b01..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/parked-car-detection.svg +++ /dev/null @@ -1,206 +0,0 @@ - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - -
-
-
- road shoulder -
-
-
- - road shoulder - - - - - - -
-
-
- Non-Avoidance Target -
- (NotParkingObject:
shiftable_ratio < threshold) -
-
-
- - Non-Avoidance Target... - - - - - - -
-
-
- Non-Avoidance Target -
- (TooNearCenterLine:
shift_length < threshold) -
-
-
- - Non-Avoidance Target... - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - -
-
-
- Avoidance Target
(shiftable_ratio > threshold)
 -
-
-
- - Avoidance Target... - - - - - - -
-
-
- Non-Avoidance Target -
- (OutOfTargetArea) -
-
-
- - Non-Avoidance Target... - - - - - - -
-
-
- Non-Avoidance Target -
- (MovingObject:
moving_time > threshold &&
velocity > threshold) -
-
-
- - Non-Avoidance Target... - - - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon.png new file mode 100644 index 0000000000000..5a627ab740cab Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon_rviz.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon_rviz.png new file mode 100644 index 0000000000000..fc6670dea98f0 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/envelope_polygon_rviz.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/lateral.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/lateral.png new file mode 100644 index 0000000000000..8d8512f042922 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/lateral.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/margin.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/margin.png new file mode 100644 index 0000000000000..8920a7be56ff9 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/margin.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/opposite_direction.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/opposite_direction.png new file mode 100644 index 0000000000000..43c72e1d5abd8 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/opposite_direction.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png new file mode 100644 index 0000000000000..1503fe58382a5 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/same_direction.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/same_direction.png new file mode 100644 index 0000000000000..acf1134c85c5c Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/same_direction.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/shift_line.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/shift_line.png new file mode 100644 index 0000000000000..9700a6ea4cb97 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/shift_line.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/avoidance.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/avoidance.png new file mode 100644 index 0000000000000..e12665b7aca81 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/avoidance.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/rviz.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/rviz.png new file mode 100644 index 0000000000000..b27e5f3d60a97 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/purpose/rviz.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/not_stop.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/not_stop.png new file mode 100644 index 0000000000000..922ed6774e9ab Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/not_stop.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/safety_check_flow.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/safety_check_flow.png new file mode 100644 index 0000000000000..206f7be73762b Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/safety_check_flow.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/stop.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/stop.png new file mode 100644 index 0000000000000..1f79a68f46f2f Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check/stop.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_flowchart.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_flowchart.svg deleted file mode 100644 index 73ad6cd59dfbc..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_flowchart.svg +++ /dev/null @@ -1,323 +0,0 @@ - - - - - - - - - - - - - -
-
-
- yes -
-
-
- - yes - - - - - - -
-
-
- passable without avoidance maneuver? -
-
-
- - passable without avoidance maneuver? - - - - - - - -
-
-
- yes -
-
-
- - yes - - - - - - -
-
-
- found avoidance path? -
-
-
- - found avoidance path? - - - - - - -
-
-
- stop in front of the target object -
-
-
- - stop in front of the target ob... - - - - - - - -
-
-
- no -
-
-
- - no - - - - - - - -
-
-
- no -
-
-
- - no - - - - - - - -
-
-
- no -
-
-
- - no - - - - - - - -
-
-
- yes -
-
-
- - yes - - - - - - -
-
-
- avoidance path is safe? -
-
-
- - avoidance path is safe? - - - - - - -
-
-
- pass through the side of the target object -
-
-
- - pass through the side of the t... - - - - - - -
-
-
- execute yield maneuver -
-
-
- - execute yield maneuver - - - - - - - -
-
-
- no -
-
-
- - no - - - - - - - -
-
-
- yes -
-
-
- - yes - - - - - - -
-
-
- avoiding now? -
-
-
- - avoiding now? - - - - - - -
-
-
- execute avoidance maneuver -
-
-
- - execute avoidance maneuver - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_margin.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_margin.svg deleted file mode 100644 index 5b49b60446027..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_margin.svg +++ /dev/null @@ -1,168 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - -
-
-
- Adjacent Lane Vehicle -
-
-
- - Adjacent Lane Vehicle - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - - -
-
-
- Adjacent Lane Vehicle -
-
-
- - Adjacent Lane Vehicle - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - - - - - -
-
-
- lateral distance -
-
-
- - lateral distance - - - - - - -
-
-
- longitudinal distance -
-
-
- - longitudinal distance - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_1.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_1.svg deleted file mode 100644 index b26805fa3c0b2..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_1.svg +++ /dev/null @@ -1,204 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - -
-
-
- Moving Vehicle -
-
-
- - Moving Vehicle - - - - - - -
-
-
- Safety Check Target -
-
-
- - Safety Check Target - - - - - - -
-
-
- center of the object does not exist on adjacent lane. -
-
-
- - center of the object does not... - - - - - - -
-
-
- ego does not shift this side. -
-
-
- - ego does not shift this side. - - - - - - - - - -
-
-
- - Moving Vehicle -
- -
-
-
- - Moving Vehicle - - - - - - - - - -
-
-
- safety_check_backward_distance - [m] -
-
-
- - safety_check_backward_distance [m] - - - - - - -
-
-
- object_check_forward_distance - [m] -
-
-
- - object_check_forward_distance [m] - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_2.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_2.svg deleted file mode 100644 index 505f61233b641..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/safety_check_step_2.svg +++ /dev/null @@ -1,143 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - -
-
-
- Adjacent Lane Vehicle -
-
-
- - Adjacent Lane Vehicle - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- ego future point -
-
-
- - ego future point - - - - - - - -
-
-
- object prediction path -
-
-
- - object prediction path - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_line_generation.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_line_generation.svg deleted file mode 100644 index 49daa6564a5fd..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/shift_line_generation.svg +++ /dev/null @@ -1,114 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - -
-
-
- Envelope Polygon -
-
-
- - Envelope Poly... - - - - - - - -
-
-
- longitudinal distance -
-
-
- - longitudinal... - - - - - - -
-
-
- lateral offset -
-
-
- - lateral offset - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_deviating.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_deviating.png new file mode 100644 index 0000000000000..437e5129fc6cf Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_deviating.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_merging.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_merging.png new file mode 100644 index 0000000000000..d7263228a0e38 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_merging.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_parallel.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_parallel.png new file mode 100644 index 0000000000000..0da7af1fdd138 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/ambiguous_parallel.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_not_on_ego_lane.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_not_on_ego_lane.png new file mode 100644 index 0000000000000..f99d26b6c1f25 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_not_on_ego_lane.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_on_ego_lane.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_on_ego_lane.png new file mode 100644 index 0000000000000..9620934857d86 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/avoid_on_ego_lane.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/crosswalk.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/crosswalk.svg new file mode 100644 index 0000000000000..4ecd939897e0a --- /dev/null +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/crosswalk.svg @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ ignore +
+
+
+ + ignore + + + + + + + + + + + + + + +
+
+
+ target +
+
+
+ + target + + + + + + + + + + + + + + + + + + + +
+
+
+ ignore +
+
+
+ + ignore + + + + + diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area.svg new file mode 100644 index 0000000000000..1390cfed01a04 --- /dev/null +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area.svg @@ -0,0 +1,272 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ backward_distance +
+
+
+ + backward_d... + + + + + + + + + + +
+
+
+ detection area +
+
+
+ + detection area + + + + + + + + + + +
+
+
+ forward_distance +
+
+
+ + forward_di... + + + + + + + + + + + + + + + +
+
+
+ vehicle_width +
+
+
+ + vehicle_wi... + + + + + + + + + + + + + + + + + + + + +
+
+
+ target +
+
+
+ + target + + + + + + + + + + +
+
+
+ ignore +
+
+
+ + ignore + + + + + + + + + + + + + +
+
+
+ target +
+
+
+ + target + + + + + diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area_rviz.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area_rviz.png new file mode 100644 index 0000000000000..adfb95b814073 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/detection_area_rviz.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/deviating.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/deviating.png new file mode 100644 index 0000000000000..8ccf1f67a685c Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/deviating.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/merging.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/merging.png new file mode 100644 index 0000000000000..e950136f2c3ec Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/merging.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_deviating.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_deviating.png new file mode 100644 index 0000000000000..e635aee9ff74d Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_deviating.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_intersection.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_intersection.png new file mode 100644 index 0000000000000..4f470bc4b4442 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_intersection.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_merging.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_merging.png new file mode 100644 index 0000000000000..6ceb068d3889f Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_merging.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_not_edge.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_not_edge.png new file mode 100644 index 0000000000000..839b704b6c2b7 Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_not_edge.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_stop_factor.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_stop_factor.png new file mode 100644 index 0000000000000..852c2835cb57e Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/never_avoid_stop_factor.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/none.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/none.png new file mode 100644 index 0000000000000..7c94bcf6c6e7c Binary files /dev/null and b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/none.png differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/parked_vehicle.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/parked_vehicle.svg new file mode 100644 index 0000000000000..e891b1d39ced6 --- /dev/null +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/target_filter/parked_vehicle.svg @@ -0,0 +1,172 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ shiftable length +
+
+
+ + shiftable... + + + + + + + + + + + + + + + + + + + + + +
+
+
+ actual shift length +
+
+
+ + actual shi... + + + + + + + + + + + + + + + +
+
+
+ vehicle width +
+
+
+ + vehicle wi... + + + + + + + + + + + + + + + +
+
+
+ lane width +
+
+
+ + lane width + + + + + diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/todo.png b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/todo.png deleted file mode 100644 index 483a3009f9b52..0000000000000 Binary files a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/todo.png and /dev/null differ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_adjacent_lane.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_adjacent_lane.svg deleted file mode 100644 index fb83c668aac35..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_adjacent_lane.svg +++ /dev/null @@ -1,79 +0,0 @@ - - - - - - - - - - - - - - - - - - - - -
-
-
- Adjacent lane -
-
-
- - Adjacent lane - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_hatched_road_markings.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_hatched_road_markings.svg deleted file mode 100644 index 0bbf48e9b56c4..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_hatched_road_markings.svg +++ /dev/null @@ -1,81 +0,0 @@ - - - - - - - - - - - - - - - - - - - - -
-
-
- Hatched road marking -
-
-
- - Hatched road marking - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_intersection_areas.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_intersection_areas.svg deleted file mode 100644 index 43391c5f85c5d..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_intersection_areas.svg +++ /dev/null @@ -1,75 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - -
-
-
- Intersection area -
-
-
- - Intersection area - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_opposite_lane.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_opposite_lane.svg deleted file mode 100644 index a7ef53443e103..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/use_opposite_lane.svg +++ /dev/null @@ -1,80 +0,0 @@ - - - - - - - - - - - - - - - - - - - - -
-
-
- Opposite lane -
-
-
- - Opposite lane - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_limitation.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_limitation.svg deleted file mode 100644 index 91f177d19ffbf..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_limitation.svg +++ /dev/null @@ -1,84 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - - - - - - -
-
-
- current shift -
-
-
- - current shift - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver.svg deleted file mode 100644 index 782ec4b306e00..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver.svg +++ /dev/null @@ -1,319 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - -
-
-
- Adjacent Lane Vehicle -
-
-
- - Adjacent Lane Vehicle - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - -
-
-
- Adjacent Lane Vehicle -
-
-
- - Adjacent Lane Vehicle - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - - - - - - - - - - - - - - -
-
-
- Decelerate to - yield_velocity - and keep the speed until the YIELD condition is removed. -
-
-
- - Decelerate to yield_velocity and keep the spee... - - - - - - - - - - - -
-
-
- Ego -
-
-
- - Ego - - - - - - -
-
-
- Adjacent Lane Vehicle -
-
-
- - Adjacent Lane Vehicle - - - - - - -
-
-
- Avoidance Target -
-
-
- - Avoidance Target - - - - - - - - - - - - - - - - - - - -
-
-
- If the YIELD condition continues and the vehicle is about to approach an obstacle without avoiding it, it will stop in front of the obstacle. -
-
-
- - If the YIELD condition continues and the vehic... - - - - - - -
-
-
- The module judge that the avoidance path is NOT safety. -
-
-
- - The module judge that the avoidance... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver_flowchart.svg b/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver_flowchart.svg deleted file mode 100644 index 03905bac7d42d..0000000000000 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/images/yield_maneuver_flowchart.svg +++ /dev/null @@ -1,333 +0,0 @@ - - - - - - - - - - - - - -
-
-
- yes -
-
-
- - yes - - - - - - -
-
-
- passable without avoidance maneuver? -
-
-
- - passable without avoidance maneuver? - - - - - - - -
-
-
- yes -
-
-
- - yes - - - - - - -
-
-
- found avoidance path? -
-
-
- - found avoidance path? - - - - - - -
-
-
- stop in front of the target object -
-
-
- - stop in front of the target ob... - - - - - - - -
-
-
- no -
-
-
- - no - - - - - - - -
-
-
- no -
-
-
- - no - - - - - - - -
-
-
- no -
-
-
- - no - - - - - - - -
-
-
- yes -
-
-
- - yes - - - - - - -
-
-
- avoidance path is safe? -
-
-
- - avoidance path is safe? - - - - - - -
-
-
- pass through the side of the target object -
-
-
- - pass through the side of the t... - - - - - - -
-
-
- execute yield maneuver -
-
-
- - execute yield maneuver - - - - - - - -
-
-
- no -
-
-
- - no - - - - - - - -
-
-
- yes -
-
-
- - yes - - - - - - -
-
-
- avoiding now? -
-
-
- - avoiding now? - - - - - - -
-
-
- execute avoidance maneuver -
-
-
- - execute avoidance maneuver - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp index a63c283704001..abbdbfdc2f1e6 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -32,11 +32,11 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { -using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; -using route_handler::Direction; +using autoware::behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using autoware::route_handler::Direction; enum class ObjectInfo { NONE = 0, @@ -59,6 +59,7 @@ enum class ObjectInfo { // unavoidable reasons NEED_DECELERATION, SAME_DIRECTION_SHIFT, + LIMIT_DRIVABLE_SPACE_TEMPORARY, INSUFFICIENT_DRIVABLE_SPACE, INSUFFICIENT_LONGITUDINAL_DISTANCE, INVALID_SHIFT_LINE, @@ -546,6 +547,8 @@ struct AvoidancePlanningData // nearest object that should be avoid std::optional stop_target_object{std::nullopt}; + std::optional red_signal_lane{std::nullopt}; + // new shift point AvoidLineArray new_shift_line{}; @@ -668,6 +671,6 @@ struct DebugData AvoidanceDebugMsgArray avoidance_debug_msg_array; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp index 85b0e1aad0ae3..043da5cbaf8cc 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp @@ -21,17 +21,17 @@ #include #include -namespace behavior_path_planner::utils::static_obstacle_avoidance +namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance { -using behavior_path_planner::AvoidanceParameters; -using behavior_path_planner::AvoidancePlanningData; -using behavior_path_planner::AvoidLineArray; -using behavior_path_planner::DebugData; -using behavior_path_planner::ObjectDataArray; -using behavior_path_planner::ObjectInfo; -using behavior_path_planner::PathShifter; -using behavior_path_planner::ShiftLineArray; +using autoware::behavior_path_planner::AvoidanceParameters; +using autoware::behavior_path_planner::AvoidancePlanningData; +using autoware::behavior_path_planner::AvoidLineArray; +using autoware::behavior_path_planner::DebugData; +using autoware::behavior_path_planner::ObjectDataArray; +using autoware::behavior_path_planner::ObjectInfo; +using autoware::behavior_path_planner::PathShifter; +using autoware::behavior_path_planner::ShiftLineArray; MarkerArray createEgoStatusMarkerArray( const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); @@ -50,14 +50,14 @@ MarkerArray createOtherObjectsMarkerArray( MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, const std::shared_ptr & parameters); -} // namespace behavior_path_planner::utils::static_obstacle_avoidance +} // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance -std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); +std::string toStrInfo(const autoware::behavior_path_planner::ShiftLineArray & sl_arr); -std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr); +std::string toStrInfo(const autoware::behavior_path_planner::AvoidLineArray & ap_arr); -std::string toStrInfo(const behavior_path_planner::ShiftLine & sl); +std::string toStrInfo(const autoware::behavior_path_planner::ShiftLine & sl); -std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); +std::string toStrInfo(const autoware::behavior_path_planner::AvoidLine & ap); #endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DEBUG_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp index 5da56a320ad3b..5d1dc168f6efe 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -26,11 +26,11 @@ #include #include -namespace behavior_path_planner::helper::static_obstacle_avoidance +namespace autoware::behavior_path_planner::helper::static_obstacle_avoidance { -using behavior_path_planner::PathShifter; -using behavior_path_planner::PlannerData; +using autoware::behavior_path_planner::PathShifter; +using autoware::behavior_path_planner::PlannerData; class AvoidanceHelper { @@ -357,6 +357,11 @@ class AvoidanceHelper return false; } + // can avoid it after relax drivable space limitation. + if (object.info == ObjectInfo::LIMIT_DRIVABLE_SPACE_TEMPORARY) { + return false; + } + return true; } @@ -513,6 +518,6 @@ class AvoidanceHelper std::optional> max_v_point_; }; -} // namespace behavior_path_planner::helper::static_obstacle_avoidance +} // namespace autoware::behavior_path_planner::helper::static_obstacle_avoidance #endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp index 1616339840ba4..f3acf0c5f3abc 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp" -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include #include @@ -25,7 +25,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface @@ -50,6 +50,6 @@ class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface std::shared_ptr parameters_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 10c0f57cfb6fe..351c686f761d7 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -24,7 +24,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; @@ -401,6 +401,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node) return p; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__PARAMETER_HELPER_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp index 57090e9f551cf..ccd00078bdec1 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include #include @@ -33,7 +33,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { using helper::static_obstacle_avoidance::AvoidanceHelper; @@ -456,6 +456,6 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp index 21eaef884b5e6..8ca24db8042e7 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp @@ -15,18 +15,18 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include -namespace behavior_path_planner::utils::static_obstacle_avoidance +namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance { -using behavior_path_planner::PathShifter; -using behavior_path_planner::helper::static_obstacle_avoidance::AvoidanceHelper; +using autoware::behavior_path_planner::PathShifter; +using autoware::behavior_path_planner::helper::static_obstacle_avoidance::AvoidanceHelper; class ShiftLineGenerator { @@ -242,6 +242,6 @@ class ShiftLineGenerator double base_offset_{0.0}; }; -} // namespace behavior_path_planner::utils::static_obstacle_avoidance +} // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance #endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp index 8bbe4b550ff00..8e4cab52a7419 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -31,7 +31,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { // auto msgs using autoware_perception_msgs::msg::PredictedObject; @@ -69,6 +69,6 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::pose2transform; using tier4_autoware_utils::toHexString; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp index 420edd17ca372..dc061457f7045 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp @@ -15,22 +15,23 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include #include -namespace behavior_path_planner::utils::static_obstacle_avoidance +namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance { -using behavior_path_planner::PlannerData; -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; -using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; +using autoware::behavior_path_planner::PlannerData; +using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using autoware::behavior_path_planner::utils::path_safety_checker:: + PoseWithVelocityAndPolygonStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; static constexpr const char * logger_namespace = "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils"; @@ -174,6 +175,6 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -} // namespace behavior_path_planner::utils::static_obstacle_avoidance +} // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance #endif // AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index 1b7814874b498..a6da4d5550562 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -21,18 +21,19 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_path_planner + autoware_behavior_path_planner_common + autoware_objects_of_interest_marker_interface autoware_perception_msgs - behavior_path_planner - behavior_path_planner_common + autoware_route_handler + autoware_rtc_interface + autoware_vehicle_info_utils geometry_msgs lanelet2_extension magic_enum motion_utils - objects_of_interest_marker_interface pluginlib rclcpp - route_handler - rtc_interface sensor_msgs signal_processing tf2 @@ -41,7 +42,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml index a4e1da9d5dccb..1efe43e80c973 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index 682e23e7f8156..a495213626e22 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include #include @@ -23,7 +23,7 @@ #include #include -namespace behavior_path_planner::utils::static_obstacle_avoidance +namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance { namespace { @@ -49,7 +49,7 @@ MarkerArray createObjectsCubeMarkerArray( MarkerArray msg; const auto is_small_object = [](const auto & o) { - const auto t = behavior_path_planner::utils::getHighestProbLabel(o.classification); + const auto t = autoware::behavior_path_planner::utils::getHighestProbLabel(o.classification); return t == ObjectClassification::PEDESTRIAN || t == ObjectClassification::BICYCLE || t == ObjectClassification::MOTORCYCLE || t == ObjectClassification::UNKNOWN; }; @@ -507,7 +507,7 @@ MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug, const std::shared_ptr & parameters) { - using behavior_path_planner::utils::transformToLanelets; + using autoware::behavior_path_planner::utils::transformToLanelets; using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::createLaneletsAreaMarkerArray; using marker_utils::createObjectsMarkerArray; @@ -653,9 +653,9 @@ MarkerArray createDebugMarkerArray( return msg; } -} // namespace behavior_path_planner::utils::static_obstacle_avoidance +} // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance -std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr) +std::string toStrInfo(const autoware::behavior_path_planner::ShiftLineArray & sl_arr) { if (sl_arr.empty()) { return "point is empty"; @@ -667,7 +667,7 @@ std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr) return ss.str(); } -std::string toStrInfo(const behavior_path_planner::ShiftLine & sl) +std::string toStrInfo(const autoware::behavior_path_planner::ShiftLine & sl) { const auto & ps = sl.start.position; const auto & pe = sl.end.position; @@ -678,7 +678,7 @@ std::string toStrInfo(const behavior_path_planner::ShiftLine & sl) return ss.str(); } -std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr) +std::string toStrInfo(const autoware::behavior_path_planner::AvoidLineArray & ap_arr) { if (ap_arr.empty()) { return "point is empty"; @@ -689,7 +689,7 @@ std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr) } return ss.str(); } -std::string toStrInfo(const behavior_path_planner::AvoidLine & ap) +std::string toStrInfo(const autoware::behavior_path_planner::AvoidLine & ap) { using tier4_autoware_utils::toHexString; diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index d43c7243fb238..b59af0fbbf0e5 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -24,7 +24,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { @@ -279,9 +279,9 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::StaticObstacleAvoidanceModuleManager, - behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager, + autoware::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 48dae73231b9f..b69508c9fdcef 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -14,15 +14,15 @@ #include "autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -34,7 +34,7 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { namespace { @@ -210,6 +210,9 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( utils::static_obstacle_avoidance::isWithinLanes(data.current_lanelets, planner_data_); const auto red_signal_lane_itr = std::find_if( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + if (utils::traffic_light::isTrafficSignalStop({lanelet}, planner_data_)) { + return true; + } const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet); return utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data_); }); @@ -229,6 +232,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( } else { data.drivable_lanes.push_back( utils::static_obstacle_avoidance::generateNotExpandedDrivableLanes(lanelet)); + data.red_signal_lane = lanelet; } }); @@ -1208,32 +1212,45 @@ bool StaticObstacleAvoidanceModule::isValidShiftLine( } } + const auto is_return_shift = + [](const double start_shift_length, const double end_shift_length, const double threshold) { + return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold; + }; + // 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_drivable_bound_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; - - const auto path = shifter_for_validate.getReferencePath(); - const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); - const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto p = getPoint(path.points.at(i)); - lanelet::BasicPoint2d basic_point{p.x, p.y}; + for (const auto & shift_line : shift_lines) { + const size_t start_idx = shift_line.start_idx; + const size_t end_idx = shift_line.end_idx; - const auto shift_length = proposed_shift_path.shift_length.at(i); - const auto THRESHOLD = minimum_distance + std::abs(shift_length); + if (is_return_shift( + shift_line.start_shift_length, shift_line.end_shift_length, + parameters_->lateral_small_shift_threshold)) { + continue; + } - if ( - boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < - THRESHOLD) { - RCLCPP_DEBUG_THROTTLE( - getLogger(), *clock_, 1000, - "following latest new shift line may cause deviation from drivable area."); - return false; + const auto path = shifter_for_validate.getReferencePath(); + const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); + const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto p = getPoint(path.points.at(i)); + lanelet::BasicPoint2d basic_point{p.x, p.y}; + + const auto shift_length = proposed_shift_path.shift_length.at(i); + const auto THRESHOLD = minimum_distance + std::abs(shift_length); + + if ( + boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < + THRESHOLD) { + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 1000, + "following latest new shift line may cause deviation from drivable area."); + return false; + } } } } @@ -1784,4 +1801,4 @@ void SceneModuleVisitor::visitAvoidanceModule(const StaticObstacleAvoidanceModul { avoidance_visitor_ = module->get_debug_msg_array(); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index f9e35ebd4c1b2..cdb89daad8adc 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -14,10 +14,10 @@ #include "autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" -namespace behavior_path_planner::utils::static_obstacle_avoidance +namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance { namespace @@ -246,7 +246,13 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidOutlines outlines; for (auto & o : data.target_objects) { if (!o.avoid_margin.has_value()) { - o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; + if (!data.red_signal_lane.has_value()) { + o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; + } else if (data.red_signal_lane.value().id() == o.overhang_lanelet.id()) { + o.info = ObjectInfo::LIMIT_DRIVABLE_SPACE_TEMPORARY; + } else { + o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; + } if (o.avoid_required && is_forward_object(o)) { break; } else { @@ -1412,4 +1418,4 @@ void ShiftLineGenerator::setRawRegisteredShiftLine( } } } -} // namespace behavior_path_planner::utils::static_obstacle_avoidance +} // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 90c8ad7b66dcd..d04a0e0281cd2 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include @@ -37,12 +37,12 @@ #include #include -namespace behavior_path_planner::utils::static_obstacle_avoidance +namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance { +using autoware::behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; +using autoware::behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; using autoware_perception_msgs::msg::TrafficLightElement; -using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; -using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; namespace { @@ -138,7 +138,7 @@ double calcSignedArcLengthToFirstNearestPoint( } geometry_msgs::msg::Polygon createVehiclePolygon( - const vehicle_info_util::VehicleInfo & vehicle_info, const double offset) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double offset) { const auto & i = vehicle_info; const auto & front_m = i.max_longitudinal_offset_m; @@ -2369,4 +2369,4 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } -} // namespace behavior_path_planner::utils::static_obstacle_avoidance +} // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index c70b78265a17b..95512fbffa575 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/behavior_path_planner_node.hpp" +#include "autoware_behavior_path_planner/behavior_path_planner_node.hpp" #include #include @@ -20,7 +20,7 @@ #include -using behavior_path_planner::BehaviorPathPlannerNode; +using autoware::behavior_path_planner::BehaviorPathPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -47,7 +47,8 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_planner"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::StaticObstacleAvoidanceModuleManager"); + module_names.emplace_back( + "autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp index da681e72533b0..110a5d39a8100 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/test/test_utils.cpp @@ -18,15 +18,16 @@ #include #include -using behavior_path_planner::ObjectData; -using behavior_path_planner::utils::static_obstacle_avoidance::isOnRight; -using behavior_path_planner::utils::static_obstacle_avoidance::isSameDirectionShift; -using behavior_path_planner::utils::static_obstacle_avoidance::isShiftNecessary; +using autoware::behavior_path_planner::ObjectData; +using autoware::behavior_path_planner::utils::static_obstacle_avoidance::isOnRight; +using autoware::behavior_path_planner::utils::static_obstacle_avoidance::isSameDirectionShift; +using autoware::behavior_path_planner::utils::static_obstacle_avoidance::isShiftNecessary; +using autoware::route_handler::Direction; TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) { ObjectData right_obj; - right_obj.direction = route_handler::Direction::RIGHT; + right_obj.direction = Direction::RIGHT; const double negative_shift_length = -1.0; const double positive_shift_length = 1.0; @@ -34,7 +35,7 @@ TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), positive_shift_length)); ObjectData left_obj; - left_obj.direction = route_handler::Direction::LEFT; + left_obj.direction = Direction::LEFT; ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), positive_shift_length)); ASSERT_FALSE(isSameDirectionShift(isOnRight(left_obj), negative_shift_length)); } @@ -42,7 +43,7 @@ TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftNecessaryTest) { ObjectData right_obj; - right_obj.direction = route_handler::Direction::RIGHT; + right_obj.direction = Direction::RIGHT; const double negative_shift_length = -1.0; const double positive_shift_length = 1.0; @@ -50,7 +51,7 @@ TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftNecessaryTest) ASSERT_FALSE(isShiftNecessary(isOnRight(right_obj), negative_shift_length)); ObjectData left_obj; - left_obj.direction = route_handler::Direction::LEFT; + left_obj.direction = Direction::LEFT; ASSERT_TRUE(isShiftNecessary(isOnRight(left_obj), negative_shift_length)); ASSERT_FALSE(isShiftNecessary(isOnRight(left_obj), positive_shift_length)); } diff --git a/planning/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt new file mode 100644 index 0000000000000..bdaa6d9f6aa2f --- /dev/null +++ b/planning/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_blind_spot_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene.cpp + src/decisions.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_blind_spot_module/README.md b/planning/autoware_behavior_velocity_blind_spot_module/README.md similarity index 100% rename from planning/behavior_velocity_blind_spot_module/README.md rename to planning/autoware_behavior_velocity_blind_spot_module/README.md diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/autoware_behavior_velocity_blind_spot_module/config/blind_spot.param.yaml similarity index 100% rename from planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml rename to planning/autoware_behavior_velocity_blind_spot_module/config/blind_spot.param.yaml diff --git a/planning/behavior_velocity_blind_spot_module/docs/blind-spot.drawio.svg b/planning/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.drawio.svg similarity index 100% rename from planning/behavior_velocity_blind_spot_module/docs/blind-spot.drawio.svg rename to planning/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.drawio.svg diff --git a/planning/behavior_velocity_blind_spot_module/docs/blind-spot.svg b/planning/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.svg similarity index 100% rename from planning/behavior_velocity_blind_spot_module/docs/blind-spot.svg rename to planning/autoware_behavior_velocity_blind_spot_module/docs/blind-spot.svg diff --git a/planning/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/autoware_behavior_velocity_blind_spot_module/package.xml new file mode 100644 index 0000000000000..8a740727445ca --- /dev/null +++ b/planning/autoware_behavior_velocity_blind_spot_module/package.xml @@ -0,0 +1,39 @@ + + + + autoware_behavior_velocity_blind_spot_module + 0.1.0 + The autoware_behavior_velocity_blind_spot_module package + + Mamoru Sobue + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Mamoru Sobue + + ament_cmake_auto + autoware_cmake + + autoware_behavior_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + geometry_msgs + lanelet2_extension + motion_utils + pluginlib + rclcpp + tf2 + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_velocity_blind_spot_module/plugins.xml b/planning/autoware_behavior_velocity_blind_spot_module/plugins.xml new file mode 100644 index 0000000000000..4e490330246d8 --- /dev/null +++ b/planning/autoware_behavior_velocity_blind_spot_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/debug.cpp b/planning/autoware_behavior_velocity_blind_spot_module/src/debug.cpp new file mode 100644 index 0000000000000..c357df481bca8 --- /dev/null +++ b/planning/autoware_behavior_velocity_blind_spot_module/src/debug.cpp @@ -0,0 +1,110 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene.hpp" + +#include +#include +#include +#include + +#include + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; + +namespace +{ + +visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( + const std::vector & polygons, const std::string & ns, + const int64_t lane_id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + int32_t i = 0; + int32_t uid = planning_utils::bitShift(lane_id); + for (const auto & polygon : polygons) { + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + + marker.ns = ns; + marker.id = uid + i++; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = createMarkerScale(0.1, 0.0, 0.0); + marker.color = createMarkerColor(r, g, b, 0.999); + for (const auto & p : polygon) { + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + marker.points.push_back(point); + } + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + msg.markers.push_back(marker); + } + + return msg; +} + +} // namespace + +motion_utils::VirtualWalls BlindSpotModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls; + + if (debug_data_.virtual_wall_pose) { + motion_utils::VirtualWall wall; + wall.text = "blind_spot"; + wall.pose = debug_data_.virtual_wall_pose.value(); + wall.ns = std::to_string(module_id_) + "_"; + virtual_walls.push_back(wall); + } + return virtual_walls; +} + +visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + const auto now = this->clock_->now(); + + if (debug_data_.detection_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.detection_area.value()}, "detection_area", module_id_, 0.5, 0.0, 0.0), + &debug_marker_array, now); + } + + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), + &debug_marker_array, now); + + return debug_marker_array; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp new file mode 100644 index 0000000000000..6b963d86c9bbf --- /dev/null +++ b/planning/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -0,0 +1,153 @@ +// Copyright 2024 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 + +namespace autoware::behavior_velocity_planner +{ + +/* + * for default + */ +template +void BlindSpotModule::setRTCStatusByDecision( + const T &, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + static_assert("Unsupported type passed to setRTCStatus"); + return; +} + +template +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const T & decision, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + static_assert("Unsupported type passed to reactRTCApprovalByDecision"); +} + +/* + * for InternalError + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + [[maybe_unused]] const InternalError & decision, + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const InternalError & decision, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + return; +} + +/* + * For OverPassJudge + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + [[maybe_unused]] const OverPassJudge & decision, + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + [[maybe_unused]] const OverPassJudge & decision, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason) +{ + return; +} + +/* + * for Unsafe + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + const Unsafe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + setSafe(false); + const auto & current_pose = planner_data_->current_odometry->pose; + setDistance( + motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason) +{ + if (!isActivated()) { + constexpr double stop_vel = 0.0; + planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose( + decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; + stop_factor.stop_pose = stop_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, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + } + return; +} + +/* + * for Safe + */ +template <> +void BlindSpotModule::setRTCStatusByDecision( + const Safe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + setSafe(true); + const auto & current_pose = planner_data_->current_odometry->pose; + setDistance( + motion_utils::calcSignedArcLength(path.points, current_pose.position, decision.stop_line_idx)); + return; +} + +template <> +void BlindSpotModule::reactRTCApprovalByDecision( + const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) +{ + if (!isActivated()) { + constexpr double stop_vel = 0.0; + planning_utils::setVelocityFromIndex(decision.stop_line_idx, stop_vel, path); + debug_data_.virtual_wall_pose = planning_utils::getAheadPose( + decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + + tier4_planning_msgs::msg::StopFactor stop_factor; + const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; + stop_factor.stop_pose = stop_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, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + } + return; +} + +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/autoware_behavior_velocity_blind_spot_module/src/manager.cpp new file mode 100644 index 0000000000000..7f2f7ebd89baa --- /dev/null +++ b/planning/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -0,0 +1,103 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "manager.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) +{ + const std::string ns(getModuleName()); + 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_detection_length = + getOrDeclareParameter(node, ns + ".backward_detection_length"); + planner_param_.ignore_width_from_center_line = + getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); + planner_param_.adjacent_extend_width = + getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + planner_param_.opposite_adjacent_extend_width = + getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); + planner_param_.max_future_movement_time = + getOrDeclareParameter(node, ns + ".max_future_movement_time"); + planner_param_.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); + planner_param_.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); + planner_param_.ttc_ego_minimal_velocity = + getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); +} + +void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & ll : planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->current_odometry->pose)) { + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + if (isModuleRegistered(module_id)) { + continue; + } + + // Is turning lane? + const std::string turn_direction_str = ll.attributeOr("turn_direction", "else"); + if (turn_direction_str != "left" && turn_direction_str != "right") { + continue; + } + const auto turn_direction = turn_direction_str == "left" + ? BlindSpotModule::TurnDirection::LEFT + : BlindSpotModule::TurnDirection::RIGHT; + + registerModule(std::make_shared( + module_id, lane_id, turn_direction, planner_data_, planner_param_, + logger_.get_child("blind_spot_module"), clock_)); + generateUUID(module_id); + updateRTCStatus( + getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), + path.header.stamp); + } +} + +std::function &)> +BlindSpotModuleManager::getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_id_set = planning_utils::getLaneIdSetOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [lane_id_set](const std::shared_ptr & scene_module) { + return lane_id_set.count(scene_module->getModuleId()) == 0; + }; +} + +} // namespace autoware::behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::BlindSpotModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/manager.hpp b/planning/autoware_behavior_velocity_blind_spot_module/src/manager.hpp new file mode 100644 index 0000000000000..756aec415c4ec --- /dev/null +++ b/planning/autoware_behavior_velocity_blind_spot_module/src/manager.hpp @@ -0,0 +1,55 @@ +// 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene.hpp" + +#include +#include +#include +#include + +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC +{ +public: + explicit BlindSpotModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "blind_spot"; } + +private: + BlindSpotModule::PlannerParam planner_param_; + + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class BlindSpotModulePlugin : public PluginWrapper +{ +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/autoware_behavior_velocity_blind_spot_module/src/scene.cpp new file mode 100644 index 0000000000000..eed767fb891f9 --- /dev/null +++ b/planning/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -0,0 +1,707 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +namespace bg = boost::geometry; + +BlindSpotModule::BlindSpotModule( + const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, + const std::shared_ptr planner_data, const PlannerParam & planner_param, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + lane_id_(lane_id), + planner_param_{planner_param}, + turn_direction_(turn_direction), + is_over_pass_judge_line_(false) +{ + velocity_factor_.init(PlanningBehavior::REAR_CHECK); + sibling_straight_lanelet_ = getSiblingStraightLanelet(planner_data); +} + +void BlindSpotModule::initializeRTCStatus() +{ + setSafe(true); + setDistance(std::numeric_limits::lowest()); +} + +BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail( + PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + if (planner_param_.use_pass_judge_line && is_over_pass_judge_line_) { + return OverPassJudge{"already over the pass judge line. no plan needed."}; + } + const auto & input_path = *path; + + /* set stop-line and stop-judgement-line for base_link */ + const auto interpolated_path_info_opt = generateInterpolatedPathInfo(input_path); + if (!interpolated_path_info_opt) { + return InternalError{"Failed to interpolate path"}; + } + const auto & interpolated_path_info = interpolated_path_info_opt.value(); + + const auto stoplines_idx_opt = generateStopLine(interpolated_path_info, path); + if (!stoplines_idx_opt) { + return InternalError{"generateStopLine fail"}; + } + + const auto [default_stopline_idx, critical_stopline_idx] = stoplines_idx_opt.value(); + if (default_stopline_idx == 0) { + return InternalError{"stop line or pass judge line is at path[0], ignore planning."}; + } + + /* calc closest index */ + const auto & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + const auto stop_line_idx = + closest_idx > default_stopline_idx ? critical_stopline_idx : default_stopline_idx; + const auto stop_line_pose = planning_utils::getAheadPose( + stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, input_path); + + const auto is_over_pass_judge = isOverPassJudge(input_path, stop_line_pose); + if (is_over_pass_judge) { + is_over_pass_judge_line_ = true; + return is_over_pass_judge.value(); + } + + if (!blind_spot_lanelets_) { + const auto blind_spot_lanelets = generateBlindSpotLanelets(input_path); + if (!blind_spot_lanelets.empty()) { + blind_spot_lanelets_ = blind_spot_lanelets; + } + } + if (!blind_spot_lanelets_) { + return InternalError{"There are not blind_spot_lanelets"}; + } + const auto & blind_spot_lanelets = blind_spot_lanelets_.value(); + + const auto detection_area_opt = generateBlindSpotPolygons( + input_path, closest_idx, blind_spot_lanelets, + path->points.at(critical_stopline_idx).point.pose); + if (!detection_area_opt) { + return InternalError{"Failed to generate BlindSpotPolygons"}; + } + const auto & detection_area = detection_area_opt.value(); + debug_data_.detection_area = detection_area; + + const auto ego_time_to_reach_stop_line = computeTimeToPassStopLine( + blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose); + /* calculate dynamic collision around detection area */ + const auto collision_obstacle = isCollisionDetected( + blind_spot_lanelets, path->points.at(critical_stopline_idx).point.pose, detection_area, + ego_time_to_reach_stop_line); + state_machine_.setStateWithMarginTime( + collision_obstacle.has_value() ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("state_machine"), *clock_); + + if (state_machine_.getState() == StateMachine::State::STOP) { + return Unsafe{stop_line_idx, collision_obstacle}; + } + + return Safe{stop_line_idx}; +} + +// template-specification based visitor pattern +// https://en.cppreference.com/w/cpp/utility/variant/visit +template +struct VisitorSwitch : Ts... +{ + using Ts::operator()...; +}; +template +VisitorSwitch(Ts...) -> VisitorSwitch; + +void BlindSpotModule::setRTCStatus( + const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + std::visit( + VisitorSwitch{[&](const auto & sub_decision) { setRTCStatusByDecision(sub_decision, path); }}, + decision); +} + +void BlindSpotModule::reactRTCApproval( + const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason) +{ + std::visit( + VisitorSwitch{[&](const auto & sub_decision) { + reactRTCApprovalByDecision(sub_decision, path, stop_reason); + }}, + decision); +} + +bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + debug_data_ = DebugData(); + *stop_reason = planning_utils::initializeStopReason(StopReason::BLIND_SPOT); + + initializeRTCStatus(); + const auto decision = modifyPathVelocityDetail(path, stop_reason); + const auto & input_path = *path; + setRTCStatus(decision, input_path); + reactRTCApproval(decision, path, stop_reason); + + return true; +} + +static bool hasLaneIds( + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) +{ + for (const auto & pid : p.lane_ids) { + if (pid == id) { + return true; + } + } + return false; +} + +static std::optional> findLaneIdInterval( + const tier4_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) +{ + bool found = false; + size_t start = 0; + size_t end = p.points.size() > 0 ? p.points.size() - 1 : 0; + if (start == end) { + // there is only one point in the path + return std::nullopt; + } + for (size_t i = 0; i < p.points.size(); ++i) { + if (hasLaneIds(p.points.at(i), id)) { + if (!found) { + // found interval for the first time + found = true; + start = i; + } + } else if (found) { + // prior point was in the interval. interval ended + end = i; + break; + } + } + start = start > 0 ? start - 1 : 0; // the idx of last point before the interval + return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt; +} + +std::optional BlindSpotModule::generateInterpolatedPathInfo( + const tier4_planning_msgs::msg::PathWithLaneId & input_path) const +{ + constexpr double ds = 0.2; + InterpolatedPathInfo interpolated_path_info; + if (!splineInterpolate(input_path, ds, interpolated_path_info.path, logger_)) { + return std::nullopt; + } + interpolated_path_info.ds = ds; + interpolated_path_info.lane_id = lane_id_; + interpolated_path_info.lane_id_interval = + findLaneIdInterval(interpolated_path_info.path, lane_id_); + return interpolated_path_info; +} + +std::optional BlindSpotModule::getSiblingStraightLanelet( + const std::shared_ptr planner_data) const +{ + const auto lanelet_map_ptr = planner_data->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data->route_handler_->getRoutingGraphPtr(); + + const auto assigned_lane = lanelet_map_ptr->laneletLayer.get(lane_id_); + for (const auto & prev : routing_graph_ptr->previous(assigned_lane)) { + for (const auto & following : routing_graph_ptr->following(prev)) { + if (std::string(following.attributeOr("turn_direction", "else")) == "straight") { + return following; + } + } + } + return std::nullopt; +} + +static std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + const auto line2d = line.basicLineString(); + for (auto i = 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, line2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + +static std::optional getDuplicatedPointIdx( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) +{ + for (size_t i = 0; i < path.points.size(); i++) { + const auto & p = path.points.at(i).point.pose.position; + + constexpr double min_dist = 0.001; + if (tier4_autoware_utils::calcDistance2d(p, point) < min_dist) { + return i; + } + } + + return std::nullopt; +} + +static std::optional insertPointIndex( + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) +{ + const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); + if (duplicate_idx_opt) { + return duplicate_idx_opt.value(); + } + + const size_t closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + // vector.insert(i) inserts element on the left side of v[i] + // the velocity need to be zero order hold(from prior point) + size_t insert_idx = closest_idx; + tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); + if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { + ++insert_idx; + } else { + // copy with velocity from prior point + const size_t prior_ind = closest_idx > 0 ? closest_idx - 1 : 0; + inserted_point.point.longitudinal_velocity_mps = + inout_path->points.at(prior_ind).point.longitudinal_velocity_mps; + } + inserted_point.point.pose = in_pose; + + auto it = inout_path->points.begin() + insert_idx; + inout_path->points.insert(it, inserted_point); + + return insert_idx; +} + +std::optional> BlindSpotModule::generateStopLine( + const InterpolatedPathInfo & interpolated_path_info, + tier4_planning_msgs::msg::PathWithLaneId * path) const +{ + // NOTE: this is optionally int for later subtraction + const int margin_idx_dist = + std::ceil(planner_param_.stop_line_margin / interpolated_path_info.ds); + + const auto & path_ip = interpolated_path_info.path; + + size_t stop_idx_default_ip = 0; + size_t stop_idx_critical_ip = 0; + if (sibling_straight_lanelet_) { + const auto sibling_straight_lanelet = sibling_straight_lanelet_.value(); + const auto turn_boundary_line = turn_direction_ == TurnDirection::LEFT + ? sibling_straight_lanelet.leftBound() + : sibling_straight_lanelet.rightBound(); + const auto first_conflict_idx_ip_opt = getFirstPointIntersectsLineByFootprint( + lanelet::utils::to2D(turn_boundary_line), interpolated_path_info, + planner_data_->vehicle_info_.createFootprint(0.0, 0.0), + planner_data_->vehicle_info_.max_longitudinal_offset_m); + if (!first_conflict_idx_ip_opt) { + return std::nullopt; + } + + // NOTE: this is optionally int for later subtraction + const auto first_conflict_idx_ip = static_cast(first_conflict_idx_ip_opt.value()); + + stop_idx_default_ip = static_cast(std::max(first_conflict_idx_ip - margin_idx_dist, 0)); + stop_idx_critical_ip = static_cast(first_conflict_idx_ip); + } else { + // the entry point of the assigned lane + const auto & assigned_lanelet = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); + const auto left_pt = assigned_lanelet.leftBound().front().basicPoint(); + const auto right_pt = assigned_lanelet.rightBound().front().basicPoint(); + const auto mid_pt = (left_pt + right_pt) / 2.0; + const geometry_msgs::msg::Point mid_point = + geometry_msgs::build().x(mid_pt.x()).y(mid_pt.y()).z(mid_pt.z()); + stop_idx_default_ip = stop_idx_critical_ip = + motion_utils::findNearestSegmentIndex(path_ip.points, mid_point); + /* + // NOTE: it is not ambiguous when the curve starts on the left/right lanelet, so this module + inserts stopline at the beginning of the lanelet for baselink + stop_idx_default_ip = stop_idx_critical_ip = static_cast(std::max(0, + static_cast(motion_utils::findNearestSegmentIndex(path_ip.points, mid_point)) - + baselink2front_dist)); + */ + } + + /* insert stop_point to use interpolated path*/ + const auto stopline_idx_default_opt = insertPointIndex( + path_ip.points.at(stop_idx_default_ip).point.pose, path, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + const auto stopline_idx_critical_opt = insertPointIndex( + path_ip.points.at(stop_idx_critical_ip).point.pose, path, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + + if (!stopline_idx_default_opt || !stopline_idx_critical_opt) { + return std::nullopt; + } + return std::make_pair(stopline_idx_default_opt.value(), stopline_idx_critical_opt.value()); +} + +autoware_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictPathWithDuration( + const std_msgs::msg::Header & header, + const autoware_perception_msgs::msg::PredictedObject & object_original, + const double time_thr) const +{ + auto object = object_original; + const rclcpp::Time current_time = clock_->now(); + + for (auto & predicted_path : 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(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); + } + } + } + return object; +} + +std::optional BlindSpotModule::isOverPassJudge( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, + const geometry_msgs::msg::Pose & stop_point_pose) const +{ + const auto & current_pose = planner_data_->current_odometry->pose; + + /* set judge line dist */ + const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithAccLimit( + planner_data_->current_velocity->twist.linear.x, planner_data_->max_stop_acceleration_threshold, + planner_data_->delay_response_time); + const auto ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + input_path.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + const size_t stop_point_segment_idx = + motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); + const auto distance_until_stop = motion_utils::calcSignedArcLength( + input_path.points, current_pose.position, ego_segment_idx, stop_point_pose.position, + stop_point_segment_idx); + + /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ + if (planner_param_.use_pass_judge_line) { + const double eps = 1e-1; // to prevent hunting + if (const auto current_state = state_machine_.getState(); + current_state == StateMachine::State::GO && + distance_until_stop + eps < pass_judge_line_dist) { + return OverPassJudge{"over the pass judge line. no plan needed."}; + } + } + return std::nullopt; +} + +double BlindSpotModule::computeTimeToPassStopLine( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose) const +{ + // egoが停止している時にそのまま速度を使うと衝突しなくなってしまうのでegoについては最低速度を使う + const auto & current_pose = planner_data_->current_odometry->pose; + const auto current_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, current_pose).length; + const auto stopline_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length; + const auto remaining_distance = stopline_arc_ego - current_arc_ego; + return remaining_distance / std::max( + planner_param_.ttc_ego_minimal_velocity, + planner_data_->current_velocity->twist.linear.x); +} + +std::optional BlindSpotModule::isCollisionDetected( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, + const double ego_time_to_reach_stop_line) +{ + // check objects in blind spot areas + const auto stop_line_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length; + for (const auto & original_object : planner_data_->predicted_objects->objects) { + if (!isTargetObjectType(original_object)) { + continue; + } + const auto object = cutPredictPathWithDuration( + planner_data_->predicted_objects->header, original_object, + planner_param_.max_future_movement_time); + // right direction + const bool exist_in_detection_area = bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); + if (!exist_in_detection_area) { + continue; + } + const auto object_arc_length = + lanelet::utils::getArcCoordinates( + blind_spot_lanelets, object.kinematics.initial_pose_with_covariance.pose) + .length; + const auto object_time_to_reach_stop_line = + (object_arc_length - stop_line_arc_ego) / + (object.kinematics.initial_twist_with_covariance.twist.linear.x); + const auto ttc = ego_time_to_reach_stop_line - object_time_to_reach_stop_line; + RCLCPP_INFO(logger_, "object ttc is %f", ttc); + if (planner_param_.ttc_min < ttc && ttc < planner_param_.ttc_max) { + return object; + } + } + return std::nullopt; +} + +lanelet::ConstLanelet BlindSpotModule::generateHalfLanelet( + const lanelet::ConstLanelet lanelet) const +{ + lanelet::Points3d lefts, rights; + + const double offset = (turn_direction_ == TurnDirection::LEFT) + ? planner_param_.ignore_width_from_center_line + : -planner_param_.ignore_width_from_center_line; + const auto offset_centerline = lanelet::utils::getCenterlineWithOffset(lanelet, offset); + + const auto original_left_bound = + (turn_direction_ == TurnDirection::LEFT) ? lanelet.leftBound() : offset_centerline; + const auto original_right_bound = + (turn_direction_ == TurnDirection::LEFT) ? offset_centerline : lanelet.rightBound(); + + for (const auto & pt : original_left_bound) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : original_right_bound) { + rights.push_back(lanelet::Point3d(pt)); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); + auto half_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + return half_lanelet; +} + +lanelet::ConstLanelet BlindSpotModule::generateExtendedAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const +{ + const auto centerline = lanelet.centerline2d(); + const auto width = + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline); + const double extend_width = std::min(planner_param_.adjacent_extend_width, width); + const auto left_bound_ = + direction == TurnDirection::LEFT + ? lanelet::utils::getCenterlineWithOffset(lanelet, -width / 2 + extend_width) + : lanelet.leftBound(); + const auto right_bound_ = + direction == TurnDirection::RIGHT + ? lanelet::utils::getCenterlineWithOffset(lanelet, width / 2 - extend_width) + : lanelet.rightBound(); + lanelet::Points3d lefts, rights; + for (const auto & pt : left_bound_) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : right_bound_) { + rights.push_back(lanelet::Point3d(pt)); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); + auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0); + new_lanelet.setCenterline(new_centerline); + return new_lanelet; +} + +lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const +{ + const auto centerline = lanelet.centerline2d(); + const auto width = + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline); + const double extend_width = + std::min(planner_param_.opposite_adjacent_extend_width, width); + const auto left_bound_ = + direction == TurnDirection::RIGHT + ? lanelet.rightBound().invert() + : lanelet::utils::getCenterlineWithOffset(lanelet.invert(), -width / 2 + extend_width); + const auto right_bound_ = + direction == TurnDirection::RIGHT + ? lanelet::utils::getCenterlineWithOffset(lanelet.invert(), width / 2 - extend_width) + : lanelet.leftBound().invert(); + lanelet::Points3d lefts, rights; + for (const auto & pt : left_bound_) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : right_bound_) { + rights.push_back(lanelet::Point3d(pt)); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); + auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0); + new_lanelet.setCenterline(new_centerline); + return new_lanelet; +} + +static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) +{ + lanelet::Points3d pts; + for (const auto & pt : line) { + pts.push_back(lanelet::Point3d(pt)); + } + return lanelet::LineString3d(lanelet::InvalId, pts); +} + +lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( + const tier4_planning_msgs::msg::PathWithLaneId & path) const +{ + /* get lanelet map */ + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + + std::vector lane_ids; + /* get lane ids until intersection */ + for (const auto & point : path.points) { + bool found_intersection_lane = false; + for (const auto lane_id : point.lane_ids) { + if (lane_id == lane_id_) { + found_intersection_lane = true; + lane_ids.push_back(lane_id); + break; + } + // make lane_ids unique + if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { + lane_ids.push_back(lane_id); + } + } + if (found_intersection_lane) break; + } + + lanelet::ConstLanelets blind_spot_lanelets; + for (const auto i : lane_ids) { + const auto lane = lanelet_map_ptr->laneletLayer.get(i); + const auto ego_half_lanelet = generateHalfLanelet(lane); + const auto adj = + turn_direction_ == TurnDirection::LEFT + ? (routing_graph_ptr->adjacentLeft(lane)) + : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) + : boost::none); + const std::optional opposite_adj = + [&]() -> std::optional { + if (!!adj) { + return std::nullopt; + } + if (turn_direction_ == TurnDirection::LEFT) { + // this should exist in right-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.leftBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); + } + if (turn_direction_ == TurnDirection::RIGHT) { + // this should exist in left-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.rightBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); + } else { + return std::nullopt; + } + }(); + + if (!adj && !opposite_adj) { + blind_spot_lanelets.push_back(ego_half_lanelet); + } else if (!!adj) { + const auto adj_half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); + const auto lefts = (turn_direction_ == TurnDirection::LEFT) ? adj_half_lanelet.leftBound() + : ego_half_lanelet.leftBound(); + const auto rights = (turn_direction_ == TurnDirection::RIGHT) ? adj_half_lanelet.rightBound() + : ego_half_lanelet.rightBound(); + blind_spot_lanelets.push_back( + lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); + } else if (opposite_adj) { + const auto adj_half_lanelet = + generateExtendedOppositeAdjacentLanelet(opposite_adj.value(), turn_direction_); + const auto lefts = (turn_direction_ == TurnDirection::LEFT) ? adj_half_lanelet.leftBound() + : ego_half_lanelet.leftBound(); + const auto rights = (turn_direction_ == TurnDirection::LEFT) ? ego_half_lanelet.rightBound() + : adj_half_lanelet.rightBound(); + blind_spot_lanelets.push_back( + lanelet::ConstLanelet(lanelet::InvalId, removeConst(lefts), removeConst(rights))); + } + } + return blind_spot_lanelets; +} + +std::optional BlindSpotModule::generateBlindSpotPolygons( + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose) const +{ + const auto stop_line_arc_ego = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose).length; + const auto detection_area_start_length_ego = + std::max(stop_line_arc_ego - planner_param_.backward_detection_length, 0.0); + return lanelet::utils::getPolygonFromArcLength( + blind_spot_lanelets, detection_area_start_length_ego, stop_line_arc_ego); +} + +bool BlindSpotModule::isTargetObjectType( + const autoware_perception_msgs::msg::PredictedObject & object) const +{ + if ( + object.classification.at(0).label == + autoware_perception_msgs::msg::ObjectClassification::BICYCLE || + object.classification.at(0).label == + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN || + object.classification.at(0).label == + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} + +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_blind_spot_module/src/scene.hpp b/planning/autoware_behavior_velocity_blind_spot_module/src/scene.hpp new file mode 100644 index 0000000000000..721576a9fac8a --- /dev/null +++ b/planning/autoware_behavior_velocity_blind_spot_module/src/scene.hpp @@ -0,0 +1,243 @@ +// 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 SCENE_HPP_ +#define SCENE_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +/** + * @brief wrapper class of interpolated path with lane id + */ +struct InterpolatedPathInfo +{ + /** the interpolated path */ + tier4_planning_msgs::msg::PathWithLaneId path; + /** discretization interval of interpolation */ + double ds{0.0}; + /** the intersection lanelet id */ + lanelet::Id lane_id{0}; + /** the range of indices for the path points with associative lane id */ + std::optional> lane_id_interval{std::nullopt}; +}; + +/** + * @brief represent action + */ +struct InternalError +{ + const std::string error; +}; + +struct OverPassJudge +{ + const std::string report; +}; + +struct Unsafe +{ + const size_t stop_line_idx; + const std::optional collision_obstacle; +}; + +struct Safe +{ + const size_t stop_line_idx; +}; + +using BlindSpotDecision = std::variant; + +class BlindSpotModule : public SceneModuleInterface +{ +public: + enum class TurnDirection { LEFT, RIGHT }; + + struct DebugData + { + std::optional virtual_wall_pose{std::nullopt}; + std::optional detection_area; + autoware_perception_msgs::msg::PredictedObjects conflicting_targets; + }; + +public: + struct PlannerParam + { + bool use_pass_judge_line; + double stop_line_margin; + double backward_detection_length; + double ignore_width_from_center_line; + double adjacent_extend_width; + double opposite_adjacent_extend_width; + double max_future_movement_time; + double ttc_min; + double ttc_max; + double ttc_ego_minimal_velocity; + }; + + BlindSpotModule( + const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, + const std::shared_ptr planner_data, const PlannerParam & planner_param, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + + /** + * @brief plan go-stop velocity at traffic crossing with collision check between reference path + * and object predicted path + */ + bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + std::vector createVirtualWalls() override; + +private: + // (semi) const variables + const int64_t lane_id_; + const PlannerParam planner_param_; + const TurnDirection turn_direction_; + std::optional sibling_straight_lanelet_{std::nullopt}; + std::optional blind_spot_lanelets_{std::nullopt}; + + // state variables + bool is_over_pass_judge_line_{false}; + + // Parameter + + void initializeRTCStatus(); + BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + // setSafe(), setDistance() + void setRTCStatus( + const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); + template + void setRTCStatusByDecision( + const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); + // stop/GO + void reactRTCApproval( + const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason); + template + void reactRTCApprovalByDecision( + const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason); + + std::optional generateInterpolatedPathInfo( + const tier4_planning_msgs::msg::PathWithLaneId & input_path) const; + + std::optional getSiblingStraightLanelet( + const std::shared_ptr planner_data) const; + + /** + * @brief Generate a stop line and insert it into the path. + * A stop line is at an intersection point of straight path with vehicle path + * @param detection_areas used to generate stop line + * @param path ego-car lane + * @param stop_line_idx generated stop line index + * @param pass_judge_line_idx generated pass judge line index + * @return false when generation failed + */ + std::optional> generateStopLine( + const InterpolatedPathInfo & interpolated_path_info, + tier4_planning_msgs::msg::PathWithLaneId * path) const; + + std::optional isOverPassJudge( + const tier4_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & stop_point_pose) const; + + double computeTimeToPassStopLine( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose) const; + + /** + * @brief Check obstacle is in blind spot areas. + * Condition1: Object's position is in broad blind spot area. + * Condition2: Object's predicted position is in narrow blind spot area. + * If both conditions are met, return true + * @param path path information associated with lane id + * @param objects_ptr dynamic objects + * @param closest_idx closest path point index from ego car in path points + * @return true when an object is detected in blind spot + */ + std::optional isCollisionDetected( + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, + const double ego_time_to_reach_stop_line); + + /** + * @brief Create half lanelet + * @param lanelet input lanelet + * @return Half lanelet + */ + lanelet::ConstLanelet generateHalfLanelet(const lanelet::ConstLanelet lanelet) const; + + lanelet::ConstLanelet generateExtendedAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; + lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; + + lanelet::ConstLanelets generateBlindSpotLanelets( + const tier4_planning_msgs::msg::PathWithLaneId & path) const; + + /** + * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. + * Broad area is made from backward expanded point to stop line point + * @param path path information associated with lane id + * @param closest_idx closest path point index from ego car in path points + * @return Blind spot polygons + */ + std::optional generateBlindSpotPolygons( + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const lanelet::ConstLanelets & blind_spot_lanelets, + const geometry_msgs::msg::Pose & pose) const; + + /** + * @brief Check if object is belong to targeted classes + * @param object Dynamic object + * @return True when object belong to targeted classes + */ + bool isTargetObjectType(const autoware_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. + * @param objects_ptr target objects + * @param time_thr time threshold to cut path + */ + autoware_perception_msgs::msg::PredictedObject cutPredictPathWithDuration( + const std_msgs::msg::Header & header, + const autoware_perception_msgs::msg::PredictedObject & object, const double time_thr) const; + + StateMachine state_machine_; //! for state + + // Debug + mutable DebugData debug_data_; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // SCENE_HPP_ diff --git a/planning/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt new file mode 100644 index 0000000000000..8447bfdedd47a --- /dev/null +++ b/planning/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_crosswalk_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY + src +) + +ament_auto_package(INSTALL_TO_SHARE config) + +install(PROGRAMS + scripts/time_to_collision_plotter.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/autoware_behavior_velocity_crosswalk_module/README.md b/planning/autoware_behavior_velocity_crosswalk_module/README.md new file mode 100644 index 0000000000000..84d3e0f77a281 --- /dev/null +++ b/planning/autoware_behavior_velocity_crosswalk_module/README.md @@ -0,0 +1,310 @@ +# Crosswalk + +## Role + +This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage for crosswalk users, such as pedestrians and bicycles, based on the objects' behavior and surround traffic. + +
+ ![crosswalk_module](docs/crosswalk_module.svg){width=1100} +
+ +## Features + +### Yield the Way to the Pedestrians + +#### Target Object + +The crosswalk module handles objects of the types defined by the following parameters in the `object_filtering.target_object` namespace. + +| Parameter | Unit | Type | Description | +| ------------ | ---- | ---- | ---------------------------------------------- | +| `unknown` | [-] | bool | whether to look and stop by UNKNOWN objects | +| `pedestrian` | [-] | bool | whether to look and stop by PEDESTRIAN objects | +| `bicycle` | [-] | bool | whether to look and stop by BICYCLE objects | +| `motorcycle` | [-] | bool | whether to look and stop by MOTORCYCLE objects | + +In order to handle the crosswalk users crossing the neighborhood but outside the crosswalk, the crosswalk module creates an attention area around the crosswalk, shown as the yellow polygon in the figure. If the object's predicted path collides with the attention area, the object will be targeted for yield. + +
+ ![crosswalk_attention_range](docs/crosswalk_attention_range.svg){width=600} +
+ +The neighborhood is defined by the following parameter in the `object_filtering.target_object` namespace. + +| Parameter | Unit | Type | Description | +| --------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------- | +| `crosswalk_attention_range` | [m] | double | the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk | + +#### Stop Position + +First of all, `stop_distance_from_object [m]` is always kept at least between the ego and the target object for safety. + +When the stop line exists in the lanelet map, the stop position is calculated based on the line. +When the stop line does **NOT** exist in the lanelet map, the stop position is calculated by keeping `stop_distance_from_crosswalk [m]` between the ego and the crosswalk. + +
+ + + + + +
+
+ +As an exceptional case, if a pedestrian (or bicycle) is crossing **wide** crosswalks seen in scramble intersections, and the pedestrian position is more than `far_object_threshold` meters away from the stop line, the actual stop position is determined by `stop_distance_from_object` and pedestrian position, not at the stop line. + +
+ ![far_object_threshold](docs/far_object_threshold.drawio.svg){width=700} +
+ +In the `stop_position` namespace, the following parameters are defined. + +| Parameter | | Type | Description | +| ------------------------------ | --- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_position_threshold` | [m] | double | If the ego vehicle has stopped near the stop line than this value, this module assumes itself to have achieved yielding. | +| `stop_distance_from_crosswalk` | [m] | double | make stop line away from crosswalk for the Lanelet2 map with no explicit stop lines | +| `far_object_threshold` | [m] | double | If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object) for the case where the crosswalk width is very wide | +| `stop_distance_from_object` | [m] | double | the vehicle decelerates to be able to stop in front of object with margin | + +#### Yield decision + +The module makes a decision to yield only when the pedestrian traffic light is **GREEN** or **UNKNOWN**. +The decision is based on the following variables, along with the calculation of the collision point. + +- Time-To-Collision (TTC): The time for the **ego** to reach the virtual collision point. +- Time-To-Vehicle (TTV): The time for the **object** to reach the virtual collision point. + +We classify ego behavior at crosswalks into three categories according to the relative relationship between TTC and TTV [1]. + +- A. **TTC >> TTV**: The object has enough time to cross before the ego. + - No stop planning. +- B. **TTC ≒ TTV**: There is a risk of collision. + - **Stop point is inserted in the ego's path**. +- C. **TTC << TTV**: Ego has enough time to cross before the object. + - No stop planning. + +
+ + + + + +
+
+ +The boundary of A and B is interpolated from `ego_pass_later_margin_x` and `ego_pass_later_margin_y`. +In the case of the upper figure, `ego_pass_later_margin_x` is `{0, 1, 2}` and `ego_pass_later_margin_y` is `{1, 4, 6}`. +In the same way, the boundary of B and C is calculated from `ego_pass_first_margin_x` and `ego_pass_first_margin_y`. +In the case of the upper figure, `ego_pass_first_margin_x` is `{3, 5}` and `ego_pass_first_margin_y` is `{0, 1}`. + +If the red signal is indicating to the corresponding crosswalk, the ego do not yield against the pedestrians. + +
+ + + + + +
+
+ +In the `pass_judge` namespace, the following parameters are defined. + +| Parameter | | Type | Description | +| ---------------------------------- | ----- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `ego_pass_first_margin_x` | [[s]] | double | time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) | +| `ego_pass_first_margin_y` | [[s]] | double | time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) | +| `ego_pass_first_additional_margin` | [s] | double | additional time margin for ego pass first situation to suppress chattering | +| `ego_pass_later_margin_x` | [[s]] | double | 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` | [[s]] | double | 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` | [s] | double | additional time margin for object pass first situation to suppress chattering | + +#### Smooth Yield Decision + +If the object is stopped near the crosswalk but has no intention of walking, a situation can arise in which the ego continues to yield the right-of-way to the object. +To prevent such a deadlock situation, the ego will cancel yielding depending on the situation. + +For the object stopped around the crosswalk but has no intention to walk (\*1), after the ego has keep stopping to yield for a specific time (\*2), the ego cancels the yield and starts driving. + +\*1: +The time is calculated by the interpolation of distance between the object and crosswalk with `distance_set_for_no_intention_to_walk` and `timeout_set_for_no_intention_to_walk`. + +In the `pass_judge` namespace, the following parameters are defined. + +| Parameter | | Type | Description | +| --------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------- | +| `distance_set_for_no_intention_to_walk` | [[m]] | double | key sets to calculate the timeout for no intention to walk with interpolation | +| `timeout_set_for_no_intention_to_walk` | [[s]] | double | value sets to calculate the timeout for no intention to walk with interpolation | + +\*2: +In the `pass_judge` namespace, the following parameters are defined. + +| Parameter | | Type | Description | +| ---------------------------- | --- | ------ | ----------------------------------------------------------------------------------------------------------------------- | +| `timeout_ego_stop_for_yield` | [s] | double | If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. | + +#### New Object Handling + +Due to the perception's limited performance where the tree or poll is recognized as a pedestrian or the tracking failure in the crowd or occlusion, even if the surrounding environment does not change, the new pedestrian (= the new ID's pedestrian) may suddenly appear unexpectedly. +If this happens while the ego is going to pass the crosswalk, the ego will stop suddenly. + +To deal with this issue, the option `disable_yield_for_new_stopped_object` is prepared. +If true is set, the yield decisions around the crosswalk with a traffic light will ignore the new stopped object. + +In the `pass_judge` namespace, the following parameters are defined. + +| Parameter | | Type | Description | +| -------------------------------------- | --- | ---- | ------------------------------------------------------------------------------------------------ | +| `disable_yield_for_new_stopped_object` | [-] | bool | If set to true, the new stopped object will be ignored around the crosswalk with a traffic light | + +### Stuck Prevention on the Crosswalk + +The feature will make the ego not to stop on the crosswalk. +When there is a low-speed or stopped vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle, the crosswalk module plans to stop before the crosswalk even if there are no pedestrians or bicycles. + +`min_acc`, `min_jerk`, and `max_jerk` are met. If the ego cannot stop before the crosswalk with these parameters, the stop position will move forward. + +
+ ![stuck_vehicle_attention_range](docs/stuck_vehicle_detection.svg){width=600} +
+ +In the `stuck_vehicle` namespace, the following parameters are defined. + +| Parameter | Unit | Type | Description | +| ---------------------------------- | ------- | ------ | ----------------------------------------------------------------------- | +| `stuck_vehicle_velocity` | [m/s] | double | maximum velocity threshold whether the target vehicle is stopped or not | +| `max_stuck_vehicle_lateral_offset` | [m] | double | maximum lateral offset of the target vehicle position | +| `required_clearance` | [m] | double | clearance to be secured between the ego and the ahead vehicle | +| `min_acc` | [m/ss] | double | minimum acceleration to stop | +| `min_jerk` | [m/sss] | double | minimum jerk to stop | +| `max_jerk` | [m/sss] | double | maximum jerk to stop | + +### Safety Slow Down Behavior + +In the current autoware implementation, if no target object is detected around a crosswalk, the ego vehicle will not slow down for the crosswalk. +However, it may be desirable to slow down in situations, for example, where there are blind spots. +Such a situation can be handled by setting some tags to the related crosswalk as instructed in the [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) +document. + +| Parameter | | Type | Description | +| --------------------- | ------- | ------ | --------------------------------------------------------------------------------------------------------------------- | +| `slow_velocity` | [m/s] | double | target vehicle velocity when module receive slow down command from FOA | +| `max_slow_down_jerk` | [m/sss] | double | minimum jerk deceleration for safe brake | +| `max_slow_down_accel` | [m/ss] | double | minimum accel deceleration for safe brake | +| `no_relax_velocity` | [m/s] | double | if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | + +### Occlusion + +This feature makes ego slow down for a crosswalk that is occluded. + +Occlusion of the crosswalk is determined using the occupancy grid. +An occlusion is a square of size `min_size` of occluded cells +(i.e., their values are between `free_space_max` and `occupied_min`) +of size `min_size`. +If an occlusion is found within range of the crosswalk, +then the velocity limit at the crosswalk is set to `slow_down_velocity` (or more to not break limits set by `max_slow_down_jerk` and `max_slow_down_accel`). +The range is calculated from the intersection between the ego path and the crosswalk and is equal to the time taken by ego to reach the crosswalk times the `occluded_object_velocity`. +This range is meant to be large when ego is far from the crosswalk and small when ego is close. + +In order to avoid flickering decisions, a time buffer can be used such that the decision to add (or remove) the slow down is only taken +after an occlusion is detected (or not detected) for a consecutive time defined by the `time_buffer` parameter. + +To ignore occlusions when the crosswalk has a traffic light, `ignore_with_traffic_light` should be set to true. + +To ignore temporary occlusions caused by moving objects, +`ignore_behind_predicted_objects` should be set to true. +By default, occlusions behind an object with velocity higher than `ignore_velocity_thresholds.default` are ignored. +This velocity threshold can be specified depending on the object type by specifying the object class label and velocity threshold in the parameter lists `ignore_velocity_thresholds.custom_labels` and `ignore_velocity_thresholds.custom_thresholds`. +To inflate the masking behind objects, their footprint can be made bigger using `extra_predicted_objects_size`. + +
+ ![stuck_vehicle_attention_range](docs/with_occlusion.svg){width=600} +
+ +| Parameter | Unit | Type | Description | +| ---------------------------------------------- | ----- | ----------- | ------------------------------------------------------------------------------------------------------------------------------------------ | +| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded | +| `occluded_object_velocity` | [m/s] | double | assumed velocity of objects that may come out of the occluded space | +| `slow_down_velocity` | [m/s] | double | slow down velocity | +| `time_buffer` | [s] | double | consecutive time with/without an occlusion to add/remove the slowdown | +| `min_size` | [m] | double | minimum size of an occlusion (square side size) | +| `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | +| `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | +| `ignore_with_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | +| `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | +| `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | +| `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_perception_msgs::msg::ObjectClassification` for all the labels) | +| `ignore_velocity_thresholds.custom_thresholds` | [-] | double list | velocities of the custom labels | +| `extra_predicted_objects_size` | [m] | double | extra size added to the objects for masking the occlusions | + +### Others + +In the `common` namespace, the following parameters are defined. + +| Parameter | Unit | Type | Description | +| ----------------------------- | ---- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `show_processing_time` | [-] | bool | whether to show processing time | +| `traffic_light_state_timeout` | [s] | double | timeout threshold for traffic light signal | +| `enable_rtc` | [-] | bool | 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. | + +## Known Issues + +- The yield decision may be sometimes aggressive or conservative depending on the case. + - The main reason is that the crosswalk module does not know the ego's position in the future. The detailed ego's position will be determined after the whole planning. + - Currently the module assumes that the ego will move with a constant velocity. + +## Debugging + +### Visualization of debug markers + +`/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk` shows the following markers. + +
+ ![limitation](docs/debug_markers.png){width=1000} +
+ +- Yellow polygons + - Ego footprints' polygon to calculate the collision check. +- Pink polygons + - Object footprints' polygon to calculate the collision check. +- The color of crosswalks + - Considering the traffic light's color, red means the target crosswalk, and white means the ignored crosswalk. +- Texts + - It shows the module ID, TTC, TTV, and the module state. + +### Visualization of Time-To-Collision + +```sh +ros2 run autoware_behavior_velocity_crosswalk_module time_to_collision_plotter.py +``` + +enables you to 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} +
+ +## Trouble Shooting + +### Behavior + +- Q. The ego stopped around the crosswalk even though there were no crosswalk user objects. + - A. See [Stuck Vehicle Detection](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/autoware_behavior_velocity_crosswalk_module/#stuck-vehicle-detection). +- Q. The crosswalk virtual wall suddenly appeared resulting in the sudden stop. + - A. There may be a crosswalk user started moving when the ego was close to the crosswalk. +- Q. The crosswalk module decides to stop even when the pedestrian traffic light is red. + - A. The lanelet map may be incorrect. The pedestrian traffic light and the crosswalk have to be related. +- Q. In the planning simulation, the crosswalk module does the yield decision to stop on all the crosswalks. + - A. This is because the pedestrian traffic light is unknown by default. In this case, the crosswalk does the yield decision for safety. + +### Parameter Tuning + +- Q. The ego's yield behavior is too conservative. + - A. Tune `ego_pass_later_margin` described in [Yield Decision](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/autoware_behavior_velocity_crosswalk_module/#stuck-vehicle-detection) +- Q. The ego's yield behavior is too aggressive. + - A. Tune `ego_pass_later_margin` described in [Yield Decision](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/autoware_behavior_velocity_crosswalk_module/#stuck-vehicle-detection) + +## References/External links + +[1] 佐藤 みなみ, 早坂 祥一, 清水 政行, 村野 隆彦, 横断歩行者に対するドライバのリスク回避行動のモデル化, 自動車技術会論文集, 2013, 44 巻, 3 号, p. 931-936. diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml similarity index 100% rename from planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml rename to planning/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml diff --git a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_module.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/crosswalk_module.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/debug_markers.png b/planning/autoware_behavior_velocity_crosswalk_module/docs/debug_markers.png similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/debug_markers.png rename to planning/autoware_behavior_velocity_crosswalk_module/docs/debug_markers.png diff --git a/planning/behavior_velocity_crosswalk_module/docs/example.png b/planning/autoware_behavior_velocity_crosswalk_module/docs/example.png similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/example.png rename to planning/autoware_behavior_velocity_crosswalk_module/docs/example.png diff --git a/planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png b/planning/autoware_behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png rename to planning/autoware_behavior_velocity_crosswalk_module/docs/time_to_collision_plot.png diff --git a/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/ttc-ttv.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/ttc-ttv.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/with_occlusion.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/with_occlusion.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/with_traffic_light.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/with_traffic_light.svg diff --git a/planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg b/planning/autoware_behavior_velocity_crosswalk_module/docs/without_traffic_light.svg similarity index 100% rename from planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg rename to planning/autoware_behavior_velocity_crosswalk_module/docs/without_traffic_light.svg diff --git a/planning/autoware_behavior_velocity_crosswalk_module/include/autoware_behavior_velocity_crosswalk_module/util.hpp b/planning/autoware_behavior_velocity_crosswalk_module/include/autoware_behavior_velocity_crosswalk_module/util.hpp new file mode 100644 index 0000000000000..ef7a8217fc6e6 --- /dev/null +++ b/planning/autoware_behavior_velocity_crosswalk_module/include/autoware_behavior_velocity_crosswalk_module/util.hpp @@ -0,0 +1,113 @@ +// 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 AUTOWARE_BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::StopFactor; + +enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; + +struct CollisionPoint +{ + geometry_msgs::msg::Point collision_point{}; + std::optional crosswalk_passage_direction{ + std::nullopt}; // denote obj is passing the crosswalk along the vehicle lane + double time_to_collision{}; + double time_to_vehicle{}; +}; + +struct DebugData +{ + DebugData() = default; + explicit DebugData(const std::shared_ptr planner_data) + : base_link2front(planner_data->vehicle_info_.max_longitudinal_offset_m) + { + } + + bool ignore_crosswalk{false}; + double base_link2front; + double stop_judge_range{}; + std::string virtual_wall_suffix{}; + + geometry_msgs::msg::Pose first_stop_pose; + geometry_msgs::msg::Point nearest_collision_point; + + std::optional range_near_point{std::nullopt}; + std::optional range_far_point{std::nullopt}; + + std::vector> collision_points; + + std::vector stop_poses; + std::vector slow_poses; + std::vector stop_factor_points; + std::vector crosswalk_polygon; + std::vector> ego_polygons; + std::vector> obj_polygons; +}; + +std::vector> getCrosswalksOnPath( + const geometry_msgs::msg::Pose & current_pose, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs); + +std::set getCrosswalkIdSetOnPath( + const geometry_msgs::msg::Pose & current_pose, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs); + +bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr); + +std::vector getPolygonIntersects( + const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, + const geometry_msgs::msg::Point & ego_pos, const size_t max_num); + +std::vector getLinestringIntersects( + const PathWithLaneId & ego_path, const lanelet::BasicLineString2d & linestring, + const geometry_msgs::msg::Point & ego_pos, const size_t max_num); + +std::optional getStopLineFromMap( + const lanelet::Id lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const std::string & attribute_name); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE_BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ diff --git a/planning/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/autoware_behavior_velocity_crosswalk_module/package.xml new file mode 100644 index 0000000000000..c3cf1339bbe6a --- /dev/null +++ b/planning/autoware_behavior_velocity_crosswalk_module/package.xml @@ -0,0 +1,54 @@ + + + + autoware_behavior_velocity_crosswalk_module + 0.1.0 + The autoware_behavior_velocity_crosswalk_module package + + Satoshi Ota + Tomoya Kimura + Shumpei Wakabayashi + Takayuki Murooka + Kyoichi Sugahara + Mamoru Sobue + Yuki Takagi + + Apache License 2.0 + + Satoshi Ota + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_vehicle_info_utils + eigen + geometry_msgs + grid_map_core + grid_map_ros + grid_map_utils + interpolation + lanelet2_extension + libboost-dev + motion_utils + nav_msgs + pcl_conversions + pluginlib + rclcpp + sensor_msgs + tier4_api_msgs + tier4_autoware_utils + tier4_debug_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_velocity_crosswalk_module/plugins.xml b/planning/autoware_behavior_velocity_crosswalk_module/plugins.xml new file mode 100644 index 0000000000000..9972d3a71b0de --- /dev/null +++ b/planning/autoware_behavior_velocity_crosswalk_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py b/planning/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py similarity index 100% rename from planning/behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py rename to planning/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/debug.cpp b/planning/autoware_behavior_velocity_crosswalk_module/src/debug.cpp new file mode 100644 index 0000000000000..1418d46081fcd --- /dev/null +++ b/planning/autoware_behavior_velocity_crosswalk_module/src/debug.cpp @@ -0,0 +1,210 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene_crosswalk.hpp" + +#include +#include +#include +#include + +#include + +namespace autoware::behavior_velocity_planner +{ + +using motion_utils::createSlowDownVirtualWallMarker; +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::createMarkerScale; +using tier4_autoware_utils::createPoint; +using visualization_msgs::msg::Marker; + +namespace +{ +visualization_msgs::msg::MarkerArray createCrosswalkMarkers( + const DebugData & debug_data, const rclcpp::Time & now, const int64_t module_id) +{ + visualization_msgs::msg::MarkerArray msg; + int32_t uid = planning_utils::bitShift(module_id); + + { + 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 = 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, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + std::ostringstream string_stream; + string_stream << std::fixed << std::setprecision(2); + string_stream << "(module, ttc, ttv, state)=(" << module_id << " , " + << point.time_to_collision << " , " << point.time_to_vehicle; + switch (state) { + case CollisionState::YIELD: + string_stream << " , YIELD)"; + break; + case CollisionState::EGO_PASS_FIRST: + string_stream << " , EGO_PASS_FIRST)"; + break; + case CollisionState::EGO_PASS_LATER: + string_stream << " , EGO_PASS_LATER)"; + break; + case CollisionState::IGNORE: + string_stream << " , IGNORE)"; + break; + default: + string_stream << " , NONE)"; + break; + } + marker.text = string_stream.str(); + marker.pose.position = point.collision_point; + marker.pose.position.z += 2.0 + i * 0.5; // NOTE: so that the texts will not overlap + msg.markers.push_back(marker); + } + } + + if (debug_data.range_near_point) { + auto marker = createDefaultMarker( + "map", now, "attention range near", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), + createMarkerColor(0.0, 0.0, 1.0, 0.999)); + marker.points.push_back(*debug_data.range_near_point); + msg.markers.push_back(marker); + } + + if (debug_data.range_far_point) { + auto marker = createDefaultMarker( + "map", now, "attention range far", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.points.push_back(*debug_data.range_far_point); + msg.markers.push_back(marker); + } + + 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), + createMarkerColor(1.0, 0.0, 1.0, 0.999)); + marker.points = points; + // marker.points.push_back(marker.points.front()); + msg.markers.push_back(marker); + } + + 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)); + marker.points = points; + // marker.points.push_back(marker.points.front()); + msg.markers.push_back(marker); + } + + // Crosswalk polygon + if (!debug_data.crosswalk_polygon.empty()) { + auto marker = createDefaultMarker( + "map", now, "crosswalk polygon", uid, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + for (const auto & p : debug_data.crosswalk_polygon) { + marker.points.push_back(createPoint(p.x, p.y, p.z)); + } + marker.points.push_back(marker.points.front()); + marker.color = debug_data.ignore_crosswalk ? createMarkerColor(1.0, 1.0, 1.0, 0.999) + : createMarkerColor(1.0, 0.0, 0.0, 0.999); + msg.markers.push_back(marker); + } + + // Collision point + if (!debug_data.collision_points.empty()) { + auto marker = createDefaultMarker( + "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(std::get<1>(collision_point).collision_point); + } + msg.markers.push_back(marker); + } + + // Slow point + if (!debug_data.slow_poses.empty()) { + auto marker = createDefaultMarker( + "map", now, "slow point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), + createMarkerColor(1.0, 1.0, 0.0, 0.999)); + for (const auto & p : debug_data.slow_poses) { + marker.points.push_back(createPoint(p.position.x, p.position.y, p.position.z)); + } + msg.markers.push_back(marker); + } + + // Stop factor point + if (!debug_data.stop_factor_points.empty()) { + auto marker = createDefaultMarker( + "map", now, "stop factor point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), + createMarkerColor(0.0, 0.0, 1.0, 0.999)); + for (const auto & p : debug_data.stop_factor_points) { + marker.points.push_back(p); + } + msg.markers.push_back(marker); + } + + // Stop point + if (!debug_data.stop_poses.empty()) { + auto marker = createDefaultMarker( + "map", now, "stop point", uid, Marker::POINTS, createMarkerScale(0.25, 0.25, 0.0), + createMarkerColor(1.0, 0.0, 0.0, 0.999)); + for (const auto & p : debug_data.stop_poses) { + marker.points.push_back(createPoint(p.position.x, p.position.y, p.position.z)); + } + msg.markers.push_back(marker); + } + + return msg; +} +} // namespace + +motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; + wall.text = "crosswalk"; + wall.ns = std::to_string(module_id_) + "_"; + + wall.style = motion_utils::VirtualWallType::stop; + for (const auto & p : debug_data_.stop_poses) { + wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + virtual_walls.push_back(wall); + } + wall.style = motion_utils::VirtualWallType::slowdown; + wall.text += debug_data_.virtual_wall_suffix; + for (const auto & p : debug_data_.slow_poses) { + wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + virtual_walls.push_back(wall); + } + return virtual_walls; +} + +visualization_msgs::msg::MarkerArray CrosswalkModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + appendMarkerArray( + createCrosswalkMarkers(debug_data_, this->clock_->now(), module_id_), &debug_marker_array); + + return debug_marker_array; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/autoware_behavior_velocity_crosswalk_module/src/manager.cpp new file mode 100644 index 0000000000000..428ec2510c7f4 --- /dev/null +++ b/planning/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -0,0 +1,237 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "manager.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +using lanelet::autoware::Crosswalk; +using tier4_autoware_utils::getOrDeclareParameter; + +CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".common.enable_rtc")) +{ + const std::string ns(getModuleName()); + + // for crosswalk parameters + auto & cp = crosswalk_planner_param_; + cp.show_processing_time = getOrDeclareParameter(node, ns + ".common.show_processing_time"); + + // param for input data + cp.traffic_light_state_timeout = + getOrDeclareParameter(node, ns + ".common.traffic_light_state_timeout"); + + // param for stop position + cp.stop_distance_from_crosswalk = + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_crosswalk"); + cp.stop_distance_from_object = + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object"); + cp.far_object_threshold = + getOrDeclareParameter(node, ns + ".stop_position.far_object_threshold"); + cp.stop_position_threshold = + getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); + + // param for ego velocity + cp.min_slow_down_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.enable_stuck_check_in_intersection = + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_stuck_check_in_intersection"); + cp.stuck_vehicle_velocity = + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); + cp.max_stuck_vehicle_lateral_offset = + getOrDeclareParameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); + cp.required_clearance = + getOrDeclareParameter(node, ns + ".stuck_vehicle.required_clearance"); + 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 = + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_first_margin_x"); + cp.ego_pass_first_margin_y = + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_first_margin_y"); + cp.ego_pass_first_additional_margin = + getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_first_additional_margin"); + cp.ego_pass_later_margin_x = + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_x"); + cp.ego_pass_later_margin_y = + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_y"); + cp.ego_pass_later_additional_margin = + getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_later_additional_margin"); + cp.ego_min_assumed_speed = + getOrDeclareParameter(node, ns + ".pass_judge.ego_min_assumed_speed"); + 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 = + getOrDeclareParameter(node, ns + ".pass_judge.stop_object_velocity_threshold"); + cp.min_object_velocity = + getOrDeclareParameter(node, ns + ".pass_judge.min_object_velocity"); + cp.disable_yield_for_new_stopped_object = + getOrDeclareParameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); + cp.distance_set_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.distance_set_for_no_intention_to_walk"); + cp.timeout_set_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.timeout_set_for_no_intention_to_walk"); + cp.timeout_ego_stop_for_yield = + getOrDeclareParameter(node, ns + ".pass_judge.timeout_ego_stop_for_yield"); + + // param for target area & object + cp.crosswalk_attention_range = + getOrDeclareParameter(node, ns + ".object_filtering.crosswalk_attention_range"); + cp.vehicle_object_cross_angle_threshold = getOrDeclareParameter( + node, ns + ".object_filtering.vehicle_object_cross_angle_threshold"); + 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 = + getOrDeclareParameter(node, ns + ".object_filtering.target_object.motorcycle"); + cp.look_pedestrian = + getOrDeclareParameter(node, ns + ".object_filtering.target_object.pedestrian"); + + // param for occlusions + cp.occlusion_enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + cp.occlusion_occluded_object_velocity = + getOrDeclareParameter(node, ns + ".occlusion.occluded_object_velocity"); + cp.occlusion_slow_down_velocity = + getOrDeclareParameter(node, ns + ".occlusion.slow_down_velocity"); + cp.occlusion_time_buffer = getOrDeclareParameter(node, ns + ".occlusion.time_buffer"); + cp.occlusion_min_size = getOrDeclareParameter(node, ns + ".occlusion.min_size"); + cp.occlusion_free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + cp.occlusion_ignore_with_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); + cp.occlusion_ignore_behind_predicted_objects = + getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); + + cp.occlusion_ignore_velocity_thresholds.resize( + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, + getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_thresholds.default")); + const auto get_label = [](const std::string & s) { + if (s == "CAR") return autoware_perception_msgs::msg::ObjectClassification::CAR; + if (s == "TRUCK") return autoware_perception_msgs::msg::ObjectClassification::TRUCK; + if (s == "BUS") return autoware_perception_msgs::msg::ObjectClassification::BUS; + if (s == "TRAILER") return autoware_perception_msgs::msg::ObjectClassification::TRAILER; + if (s == "MOTORCYCLE") return autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE; + if (s == "BICYCLE") return autoware_perception_msgs::msg::ObjectClassification::BICYCLE; + if (s == "PEDESTRIAN") return autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; + }; + const auto custom_labels = getOrDeclareParameter>( + node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels"); + const auto custom_velocities = getOrDeclareParameter>( + node, ns + ".occlusion.ignore_velocity_thresholds.custom_thresholds"); + for (auto idx = 0UL; idx < std::min(custom_labels.size(), custom_velocities.size()); ++idx) { + cp.occlusion_ignore_velocity_thresholds[get_label(custom_labels[idx])] = custom_velocities[idx]; + } + cp.occlusion_extra_objects_size = + getOrDeclareParameter(node, ns + ".occlusion.extra_predicted_objects_size"); +} + +void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) +{ + const auto rh = planner_data_->route_handler_; + + const auto launch = [this, &path]( + const auto road_lanelet_id, const auto crosswalk_lanelet_id, + const std::optional & reg_elem_id) { + if (isModuleRegistered(crosswalk_lanelet_id)) { + return; + } + + const auto & p = crosswalk_planner_param_; + 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( + node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, + clock_)); + generateUUID(crosswalk_lanelet_id); + updateRTCStatus( + getUUID(crosswalk_lanelet_id), true, State::WAITING_FOR_EXECUTION, + std::numeric_limits::lowest(), path.header.stamp); + }; + + const auto crosswalk_reg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_reg_elem_map) { + // NOTE: The former id is a lane id, and the latter one is a regulatory element's id. + launch(crosswalk.second.id(), crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id()); + } + + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk.first, crosswalk.second.id(), std::nullopt); + } +} + +std::function &)> +CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) +{ + const auto rh = planner_data_->route_handler_; + + std::set crosswalk_id_set; + + crosswalk_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + const auto crosswalk_reg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_reg_elem_map) { + crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); + } + + return [crosswalk_id_set](const std::shared_ptr & scene_module) { + return crosswalk_id_set.count(scene_module->getModuleId()) == 0; + }; +} +} // namespace autoware::behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::CrosswalkModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/manager.hpp b/planning/autoware_behavior_velocity_crosswalk_module/src/manager.hpp new file mode 100644 index 0000000000000..520dab49b347f --- /dev/null +++ b/planning/autoware_behavior_velocity_crosswalk_module/src/manager.hpp @@ -0,0 +1,61 @@ +// 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene_crosswalk.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +using tier4_planning_msgs::msg::PathWithLaneId; + +class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC +{ +public: + explicit CrosswalkModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "crosswalk"; } + +private: + CrosswalkModule::PlannerParam crosswalk_planner_param_{}; + + void launchNewModules(const PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const PathWithLaneId & path) override; +}; + +class CrosswalkModulePlugin : public PluginWrapper +{ +}; +} // namespace autoware::behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp similarity index 95% rename from planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp rename to planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index b6adb32f642eb..9b66802c1e5ed 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -14,7 +14,7 @@ #include "occluded_crosswalk.hpp" -#include "behavior_velocity_crosswalk_module/util.hpp" +#include "autoware_behavior_velocity_crosswalk_module/util.hpp" #include #include @@ -22,11 +22,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::Index idx_offset; for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { @@ -140,7 +140,7 @@ bool is_crosswalk_occluded( const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, const std::vector & dynamic_objects, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::GridMap grid_map; grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); @@ -170,4 +170,4 @@ double calculate_detection_range( const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity); return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp similarity index 93% rename from planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp rename to planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 4aab9d3bfc888..c4f2c1a23c57e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/autoware_behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /// @brief check if the gridmap is occluded at the given index /// @param [in] grid_map input grid map @@ -40,7 +40,7 @@ namespace behavior_velocity_planner /// @return true if the index is occluded bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief interpolate a point beyond the end of the given segment /// @param [in] segment input segment @@ -62,7 +62,7 @@ bool is_crosswalk_occluded( const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, const std::vector & dynamic_objects, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief calculate the distance away from the crosswalk that should be checked for occlusions /// @param occluded_objects_velocity assumed velocity of the objects coming out of occlusions @@ -89,6 +89,6 @@ std::vector select_and_inflate_o void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, const std::vector & objects); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp similarity index 99% rename from planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp rename to planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 191eea8feabed..0db87508975a0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -16,8 +16,8 @@ #include "occluded_crosswalk.hpp" -#include -#include +#include +#include #include #include #include @@ -38,7 +38,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcArcLength; @@ -1207,7 +1207,7 @@ geometry_msgs::msg::Polygon CrosswalkModule::createObjectPolygon( } geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon( - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const auto & i = vehicle_info; const auto & front_m = i.max_longitudinal_offset_m; @@ -1281,4 +1281,4 @@ void CrosswalkModule::planStop( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp similarity index 98% rename from planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp rename to planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 107a904dd076b..fe353b24273e6 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -15,9 +15,9 @@ #ifndef SCENE_CROSSWALK_HPP_ #define SCENE_CROSSWALK_HPP_ -#include "behavior_velocity_crosswalk_module/util.hpp" +#include "autoware_behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include @@ -46,7 +46,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using autoware_perception_msgs::msg::ObjectClassification; @@ -419,7 +419,7 @@ class CrosswalkModule : public SceneModuleInterface const double width_m, const double length_m); static geometry_msgs::msg::Polygon createVehiclePolygon( - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); void recordTime(const int step_num) { @@ -459,6 +459,6 @@ class CrosswalkModule : public SceneModuleInterface std::optional current_initial_occlusion_time_; std::optional most_recent_occlusion_time_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_CROSSWALK_HPP_ diff --git a/planning/autoware_behavior_velocity_crosswalk_module/src/util.cpp b/planning/autoware_behavior_velocity_crosswalk_module/src/util.cpp new file mode 100644 index 0000000000000..1e40429e3a704 --- /dev/null +++ b/planning/autoware_behavior_velocity_crosswalk_module/src/util.cpp @@ -0,0 +1,235 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_behavior_velocity_crosswalk_module/util.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +namespace bg = boost::geometry; +using motion_utils::calcSignedArcLength; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::Line2d; +using tier4_autoware_utils::Point2d; + +std::vector> getCrosswalksOnPath( + const geometry_msgs::msg::Pose & current_pose, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs) +{ + std::vector> crosswalks; + + // Add current lane id + const auto nearest_lane_id = + autoware::behavior_velocity_planner::planning_utils::getNearestLaneId( + path, lanelet_map, current_pose); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + // Add subsequent lane_ids from nearest lane_id + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); + } else { + // Add all lane_ids in path + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + } + + for (const auto lane_id : unique_lane_ids) { + const auto ll = lanelet_map->laneletLayer.get(lane_id); + + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID); + for (const auto & crosswalk : conflicting_crosswalks) { + crosswalks.emplace_back(lane_id, crosswalk); + } + } + + return crosswalks; +} + +std::set getCrosswalkIdSetOnPath( + const geometry_msgs::msg::Pose & current_pose, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs) +{ + std::set crosswalk_id_set; + + for (const auto & crosswalk : + getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) { + crosswalk_id_set.insert(crosswalk.second.id()); + } + + return crosswalk_id_set; +} + +bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); + return !lanelet::utils::query::crosswalks(all_lanelets).empty(); +} + +std::vector getPolygonIntersects( + const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, + const geometry_msgs::msg::Point & ego_pos, + const size_t max_num = std::numeric_limits::max()) +{ + std::vector intersects{}; + + bool found_max_num = false; + for (size_t i = 0; i < ego_path.points.size() - 1; ++i) { + const auto & p_back = ego_path.points.at(i).point.pose.position; + const auto & p_front = ego_path.points.at(i + 1).point.pose.position; + const Line2d segment{{p_back.x, p_back.y}, {p_front.x, p_front.y}}; + + std::vector tmp_intersects{}; + bg::intersection(segment, polygon, tmp_intersects); + + for (const auto & p : tmp_intersects) { + intersects.push_back(p); + if (intersects.size() == max_num) { + found_max_num = true; + break; + } + } + + if (found_max_num) { + break; + } + } + + const auto compare = [&](const Point2d & p1, const Point2d & p2) { + const auto dist_l1 = + calcSignedArcLength(ego_path.points, size_t(0), createPoint(p1.x(), p1.y(), ego_pos.z)); + + const auto dist_l2 = + calcSignedArcLength(ego_path.points, size_t(0), createPoint(p2.x(), p2.y(), ego_pos.z)); + + return dist_l1 < dist_l2; + }; + + std::sort(intersects.begin(), intersects.end(), compare); + + // convert tier4_autoware_utils::Point2d to geometry::msg::Point + std::vector geometry_points; + for (const auto & p : intersects) { + geometry_points.push_back(createPoint(p.x(), p.y(), ego_pos.z)); + } + return geometry_points; +} + +std::vector getLinestringIntersects( + const PathWithLaneId & ego_path, const lanelet::BasicLineString2d & linestring, + const geometry_msgs::msg::Point & ego_pos, + const size_t max_num = std::numeric_limits::max()) +{ + std::vector intersects{}; + + bool found_max_num = false; + for (size_t i = 0; i < ego_path.points.size() - 1; ++i) { + const auto & p_back = ego_path.points.at(i).point.pose.position; + const auto & p_front = ego_path.points.at(i + 1).point.pose.position; + const Line2d segment{{p_back.x, p_back.y}, {p_front.x, p_front.y}}; + + std::vector tmp_intersects{}; + bg::intersection(segment, linestring, tmp_intersects); + + for (const auto & p : tmp_intersects) { + intersects.push_back(p); + if (intersects.size() == max_num) { + found_max_num = true; + break; + } + } + + if (found_max_num) { + break; + } + } + + const auto compare = [&](const Point2d & p1, const Point2d & p2) { + const auto dist_l1 = + calcSignedArcLength(ego_path.points, size_t(0), createPoint(p1.x(), p1.y(), ego_pos.z)); + + const auto dist_l2 = + calcSignedArcLength(ego_path.points, size_t(0), createPoint(p2.x(), p2.y(), ego_pos.z)); + + return dist_l1 < dist_l2; + }; + + std::sort(intersects.begin(), intersects.end(), compare); + + // convert tier4_autoware_utils::Point2d to geometry::msg::Point + std::vector geometry_points; + for (const auto & p : intersects) { + geometry_points.push_back(createPoint(p.x(), p.y(), ego_pos.z)); + } + return geometry_points; +} + +std::optional getStopLineFromMap( + const lanelet::Id lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const std::string & attribute_name) +{ + lanelet::ConstLanelet lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); + const auto road_markings = lanelet.regulatoryElementsAs(); + lanelet::ConstLineStrings3d stop_line; + for (const auto & road_marking : road_markings) { + // TODO(someone): Create regulatory element for crosswalk + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + const int target_id = road_marking->roadMarking().attributeOr(attribute_name, 0); + if (type == lanelet::AttributeValueString::StopLine && target_id == lane_id) { + stop_line.push_back(road_marking->roadMarking()); + break; // only one stop_line exists. + } + } + if (stop_line.empty()) { + return {}; + } + + return stop_line.front(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt b/planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt new file mode 100644 index 0000000000000..f3b31001978e9 --- /dev/null +++ b/planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_intersection_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) + +find_package(OpenCV REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/manager.cpp + src/util.cpp + src/scene_intersection.cpp + src/intersection_lanelets.cpp + src/object_manager.cpp + src/decision_result.cpp + src/scene_intersection_prepare_data.cpp + src/scene_intersection_stuck.cpp + src/scene_intersection_occlusion.cpp + src/scene_intersection_collision.cpp + src/scene_merge_from_private_road.cpp + src/debug.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBRARIES} +) + +ament_auto_package(INSTALL_TO_SHARE config) + +install(PROGRAMS + scripts/ttc.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/autoware_behavior_velocity_intersection_module/README.md similarity index 100% rename from planning/behavior_velocity_intersection_module/README.md rename to planning/autoware_behavior_velocity_intersection_module/README.md diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml similarity index 100% rename from planning/behavior_velocity_intersection_module/config/intersection.param.yaml rename to planning/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml diff --git a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/data-structure.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/data-structure.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/merge_from_private.png b/planning/autoware_behavior_velocity_intersection_module/docs/merge_from_private.png similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/merge_from_private.png rename to planning/autoware_behavior_velocity_intersection_module/docs/merge_from_private.png diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/static-occlusion-timeout.png b/planning/autoware_behavior_velocity_intersection_module/docs/static-occlusion-timeout.png similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/static-occlusion-timeout.png rename to planning/autoware_behavior_velocity_intersection_module/docs/static-occlusion-timeout.png diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/ttc.gif b/planning/autoware_behavior_velocity_intersection_module/docs/ttc.gif similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/ttc.gif rename to planning/autoware_behavior_velocity_intersection_module/docs/ttc.gif diff --git a/planning/behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg diff --git a/planning/autoware_behavior_velocity_intersection_module/package.xml b/planning/autoware_behavior_velocity_intersection_module/package.xml new file mode 100644 index 0000000000000..4e79139ba2fe7 --- /dev/null +++ b/planning/autoware_behavior_velocity_intersection_module/package.xml @@ -0,0 +1,48 @@ + + + + autoware_behavior_velocity_intersection_module + 0.1.0 + The autoware_behavior_velocity_intersection_module package + + Mamoru Sobue + Takayuki Murooka + Tomoya Kimura + Shumpei Wakabayashi + Kyoichi Sugahara + + Apache License 2.0 + + Mamoru Sobue + + ament_cmake_auto + autoware_cmake + + autoware_behavior_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_rtc_interface + autoware_vehicle_info_utils + fmt + geometry_msgs + interpolation + lanelet2_extension + libopencv-dev + magic_enum + motion_utils + nav_msgs + pluginlib + rclcpp + tf2_geometry_msgs + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_velocity_intersection_module/plugins.xml b/planning/autoware_behavior_velocity_intersection_module/plugins.xml new file mode 100644 index 0000000000000..d30fbb46e050e --- /dev/null +++ b/planning/autoware_behavior_velocity_intersection_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/autoware_behavior_velocity_intersection_module/scripts/ttc.py similarity index 100% rename from planning/behavior_velocity_intersection_module/scripts/ttc.py rename to planning/autoware_behavior_velocity_intersection_module/scripts/ttc.py diff --git a/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp b/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp new file mode 100644 index 0000000000000..ab09ca2e6e746 --- /dev/null +++ b/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp @@ -0,0 +1,499 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "scene_intersection.hpp" +#include "scene_merge_from_private_road.hpp" + +#include +#include +#include +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +namespace +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; + +visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( + const std::vector & polygons, const std::string & ns, + const int64_t lane_id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + int32_t i = 0; + int32_t uid = autoware::behavior_velocity_planner::planning_utils::bitShift(lane_id); + for (const auto & polygon : polygons) { + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + + marker.ns = ns; + marker.id = uid + i++; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = createMarkerScale(0.1, 0.0, 0.0); + marker.color = createMarkerColor(r, g, b, 0.999); + for (const auto & p : polygon) { + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + marker.points.push_back(point); + } + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createPoseMarkerArray( + const geometry_msgs::msg::Pose & pose, 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_line{}; + marker_line.header.frame_id = "map"; + marker_line.ns = ns + "_line"; + marker_line.id = id; + marker_line.lifetime = rclcpp::Duration::from_seconds(0.3); + marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_line.action = visualization_msgs::msg::Marker::ADD; + marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker_line.scale = createMarkerScale(0.2, 0.0, 0.0); + marker_line.color = createMarkerColor(r, g, b, 0.999); + + const double yaw = tf2::getYaw(pose.orientation); + + const double a = 3.0; + geometry_msgs::msg::Point p0; + p0.x = pose.position.x - a * std::sin(yaw); + p0.y = pose.position.y + a * std::cos(yaw); + p0.z = pose.position.z; + marker_line.points.push_back(p0); + + geometry_msgs::msg::Point p1; + p1.x = pose.position.x + a * std::sin(yaw); + p1.y = pose.position.y - a * std::cos(yaw); + p1.z = pose.position.z; + marker_line.points.push_back(p1); + + msg.markers.push_back(marker_line); + + return msg; +} + +visualization_msgs::msg::MarkerArray createArrowLineMarkerArray( + 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; +} + +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::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.scale.x = 0.1; + 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) +{ + visualization_msgs::msg::Marker marker_point{}; + marker_point.header.frame_id = "map"; + marker_point.ns = ns + "_point"; + marker_point.id = id; + marker_point.lifetime = rclcpp::Duration::from_seconds(0.3); + marker_point.type = visualization_msgs::msg::Marker::SPHERE; + marker_point.action = visualization_msgs::msg::Marker::ADD; + marker_point.scale = createMarkerScale(2.0, 2.0, 2.0); + marker_point.color = createMarkerColor(r, g, b, 0.999); + + marker_point.pose.position = point; + + return marker_point; +} + +constexpr std::tuple white() +{ + constexpr uint64_t code = 0xfdfdfd; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple green() +{ + constexpr uint64_t code = 0x5fa641; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple yellow() +{ + constexpr uint64_t code = 0xebce2b; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple red() +{ + constexpr uint64_t code = 0xba1c30; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple light_blue() +{ + constexpr uint64_t code = 0x96cde6; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} +} // namespace + +namespace autoware::behavior_velocity_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; + +visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + const auto now = this->clock_->now(); + + if (debug_data_.attention_area) { + appendMarkerArray( + ::createLaneletPolygonsMarkerArray( + debug_data_.attention_area.value(), "attention_area", lane_id_, 0.0, 1.0, 0.0), + &debug_marker_array); + } + + if (debug_data_.occlusion_attention_area) { + appendMarkerArray( + ::createLaneletPolygonsMarkerArray( + debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, + 0.568, 0.596), + &debug_marker_array); + } + + if (debug_data_.adjacent_area) { + appendMarkerArray( + ::createLaneletPolygonsMarkerArray( + debug_data_.adjacent_area.value(), "adjacent_area", lane_id_, 0.913, 0.639, 0.149), + &debug_marker_array); + } + + if (debug_data_.first_attention_area) { + appendMarkerArray( + ::createLaneletPolygonsMarkerArray( + {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_attention_area) { + appendMarkerArray( + ::createLaneletPolygonsMarkerArray( + {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + + if (debug_data_.stuck_vehicle_detect_area) { + appendMarkerArray( + debug::createPolygonMarkerArray( + debug_data_.stuck_vehicle_detect_area.value(), "stuck_vehicle_detect_area", lane_id_, now, + 0.3, 0.0, 0.0, 0.0, 0.5, 0.5), + &debug_marker_array, now); + } + + if (debug_data_.yield_stuck_detect_area) { + appendMarkerArray( + ::createLaneletPolygonsMarkerArray( + debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, + 0.34509, 0.6588235), + &debug_marker_array); + } + + if (debug_data_.ego_lane) { + appendMarkerArray( + ::createLaneletPolygonsMarkerArray( + {debug_data_.ego_lane.value()}, "ego_lane", lane_id_, 1, 0.647, 0.0), + &debug_marker_array, now); + } + + if (debug_data_.candidate_collision_ego_lane_polygon) { + appendMarkerArray( + debug::createPolygonMarkerArray( + debug_data_.candidate_collision_ego_lane_polygon.value(), + "candidate_collision_ego_lane_polygon", module_id_, now, 0.3, 0.0, 0.0, 0.5, 0.0, 0.0), + &debug_marker_array, now); + } + + static constexpr auto white = ::white(); + static constexpr auto green = ::green(); + static constexpr auto yellow = ::yellow(); + static constexpr auto red = ::red(); + static constexpr auto light_blue = ::light_blue(); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.safe_under_traffic_control_targets, "safe_under_traffic_control_targets", + module_id_, now, std::get<0>(light_blue), std::get<1>(light_blue), std::get<2>(light_blue)), + &debug_marker_array, now); + + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.unsafe_targets, "unsafe_targets", module_id_, now, std::get<0>(green), + std::get<1>(green), std::get<2>(green)), + &debug_marker_array, now); + + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.misjudge_targets, "misjudge_targets", module_id_, now, std::get<0>(yellow), + std::get<1>(yellow), std::get<2>(yellow)), + &debug_marker_array, now); + + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.too_late_detect_targets, "too_late_detect_targets", module_id_, now, + std::get<0>(red), std::get<1>(red), std::get<2>(red)), + &debug_marker_array, now); + + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.parked_targets, "parked_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), + &debug_marker_array, now); + + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.stuck_targets, "stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), + &debug_marker_array, now); + + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.yield_stuck_targets, "yield_stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), + &debug_marker_array, now); + + if (debug_data_.first_pass_judge_wall_pose) { + const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0; + const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0; + appendMarkerArray( + ::createPoseMarkerArray( + debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, + g, 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_pass_judge_wall_pose) { + const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0; + const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0; + appendMarkerArray( + ::createPoseMarkerArray( + debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, + r, g, 0.0), + &debug_marker_array, now); + } + + for (size_t j = 0; j < debug_data_.occlusion_polygons.size(); ++j) { + const auto & p = debug_data_.occlusion_polygons.at(j); + appendMarkerArray( + debug::createPolygonMarkerArray( + p, "occlusion_polygons", lane_id_ + j, now, 0.3, 0.0, 0.0, 1.0, 0.0, 0.0), + &debug_marker_array, now); + } + + if (debug_data_.nearest_occlusion_projection) { + const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); + appendMarkerArray( + ::createArrowLineMarkerArray( + point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), + &debug_marker_array, now); + } + + if (debug_data_.nearest_occlusion_triangle) { + const auto [p1, p2, p3] = debug_data_.nearest_occlusion_triangle.value(); + const auto color = debug_data_.static_occlusion ? green : red; + geometry_msgs::msg::Polygon poly; + poly.points.push_back( + geometry_msgs::build().x(p1.x).y(p1.y).z(p1.z)); + poly.points.push_back( + geometry_msgs::build().x(p2.x).y(p2.y).z(p2.z)); + poly.points.push_back( + geometry_msgs::build().x(p3.x).y(p3.y).z(p3.z)); + appendMarkerArray( + debug::createPolygonMarkerArray( + poly, "nearest_occlusion_triangle", lane_id_, now, 0.3, 0.0, 0.0, std::get<0>(color), + std::get<1>(color), std::get<2>(color)), + &debug_marker_array, now); + } + if (debug_data_.traffic_light_observation) { + const auto GREEN = autoware_perception_msgs::msg::TrafficLightElement::GREEN; + const auto YELLOW = autoware_perception_msgs::msg::TrafficLightElement::AMBER; + + const auto [ego, tl_point, id, color] = debug_data_.traffic_light_observation.value(); + geometry_msgs::msg::Point tl_point_point; + tl_point_point.x = tl_point.x(); + tl_point_point.y = tl_point.y(); + tl_point_point.z = tl_point.z(); + const auto tl_color = (color == GREEN) ? green : (color == YELLOW ? yellow : red); + const auto [r, g, b] = tl_color; + appendMarkerArray( + ::createLineMarkerArray( + ego.position, tl_point_point, "intersection_traffic_light", lane_id_, r, g, b), + &debug_marker_array, now); + } + return debug_marker_array; +} + +motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; + + 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; +} + +visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + const auto state = state_machine_.getState(); + + int32_t uid = autoware::behavior_velocity_planner::planning_utils::bitShift(module_id_); + const auto now = this->clock_->now(); + if (state == StateMachine::State::STOP) { + appendMarkerArray( + ::createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), + &debug_marker_array, now); + } + + return debug_marker_array; +} + +motion_utils::VirtualWalls MergeFromPrivateRoadModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls; + const auto state = state_machine_.getState(); + if (state == StateMachine::State::STOP) { + motion_utils::VirtualWall wall; + wall.style = motion_utils::VirtualWallType::stop; + wall.pose = debug_data_.virtual_wall_pose; + wall.text = "merge_from_private_road"; + virtual_walls.push_back(wall); + } + return virtual_walls; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp similarity index 96% rename from planning/behavior_velocity_intersection_module/src/decision_result.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp index 7ed896d1b4b55..5d1f1a1fcfca2 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp @@ -14,7 +14,7 @@ #include "decision_result.hpp" -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { std::string formatDecisionResult(const DecisionResult & decision_result) { @@ -65,4 +65,4 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return ""; } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp similarity index 97% rename from planning/behavior_velocity_intersection_module/src/decision_result.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp index 5f642db3a462d..26f78616a7b61 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp @@ -19,7 +19,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -172,6 +172,6 @@ using DecisionResult = std::variant< std::string formatDecisionResult(const DecisionResult & decision_result); -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // DECISION_RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp index b6ae84aa8488b..d8bada002e37b 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -23,7 +23,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -43,6 +43,6 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // INTERPOLATED_PATH_INFO_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp similarity index 97% rename from planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp index 555ea424dfef0..be7e9e25cc8bc 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { void IntersectionLanelets::update( @@ -79,4 +79,4 @@ void IntersectionLanelets::update( } } } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp similarity index 98% rename from planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp index 9624d375de122..a7ca5eb3b0bc1 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -190,6 +190,6 @@ struct PathLanelets // conflicting lanelets plus the next lane part of the // path }; -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // INTERSECTION_LANELETS_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp index 99d79d4468b38..98325f0aa8735 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -17,7 +17,7 @@ #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -72,6 +72,6 @@ struct IntersectionStopLines */ size_t occlusion_wo_tl_pass_judge_line{0}; }; -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // INTERSECTION_STOPLINES_HPP_ diff --git a/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp new file mode 100644 index 0000000000000..8f540603791cb --- /dev/null +++ b/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -0,0 +1,600 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "manager.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using tier4_autoware_utils::getOrDeclareParameter; + +IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection")), + occlusion_rtc_interface_( + &node, "intersection_occlusion", + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) +{ + const std::string ns(getModuleName()); + auto & ip = intersection_param_; + + // common + { + ip.common.attention_area_length = + getOrDeclareParameter(node, ns + ".common.attention_area_length"); + ip.common.attention_area_margin = + getOrDeclareParameter(node, ns + ".common.attention_area_margin"); + ip.common.attention_area_angle_threshold = + getOrDeclareParameter(node, ns + ".common.attention_area_angle_threshold"); + ip.common.use_intersection_area = + getOrDeclareParameter(node, ns + ".common.use_intersection_area"); + ip.common.default_stopline_margin = + getOrDeclareParameter(node, ns + ".common.default_stopline_margin"); + ip.common.stopline_overshoot_margin = + getOrDeclareParameter(node, ns + ".common.stopline_overshoot_margin"); + ip.common.path_interpolation_ds = + getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); + 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.common.enable_pass_judge_before_default_stopline = + getOrDeclareParameter(node, ns + ".common.enable_pass_judge_before_default_stopline"); + } + + // stuck + { + // target_type + { + ip.stuck_vehicle.target_type.car = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.car"); + ip.stuck_vehicle.target_type.bus = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.bus"); + ip.stuck_vehicle.target_type.truck = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.truck"); + ip.stuck_vehicle.target_type.trailer = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.trailer"); + ip.stuck_vehicle.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.motorcycle"); + ip.stuck_vehicle.target_type.bicycle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.bicycle"); + ip.stuck_vehicle.target_type.unknown = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.unknown"); + } + + // turn_direction + { + 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 = + getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); + ip.stuck_vehicle.stuck_vehicle_detect_dist = + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); + ip.stuck_vehicle.stuck_vehicle_velocity_threshold = + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); + ip.stuck_vehicle.disable_against_private_lane = + getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); + } + + // yield_stuck + { + // target_type + { + ip.yield_stuck.target_type.car = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.car"); + ip.yield_stuck.target_type.bus = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.bus"); + ip.yield_stuck.target_type.truck = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.truck"); + ip.yield_stuck.target_type.trailer = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.trailer"); + ip.yield_stuck.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.motorcycle"); + ip.yield_stuck.target_type.bicycle = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.bicycle"); + ip.yield_stuck.target_type.unknown = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.unknown"); + } + + // turn_direction + { + ip.yield_stuck.turn_direction.left = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); + ip.yield_stuck.turn_direction.right = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.right"); + ip.yield_stuck.turn_direction.straight = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.straight"); + } + + ip.yield_stuck.distance_threshold = + getOrDeclareParameter(node, ns + ".yield_stuck.distance_threshold"); + } + + // collision_detection + { + ip.collision_detection.consider_wrong_direction_vehicle = getOrDeclareParameter( + node, ns + ".collision_detection.consider_wrong_direction_vehicle"); + ip.collision_detection.collision_detection_hold_time = getOrDeclareParameter( + node, ns + ".collision_detection.collision_detection_hold_time"); + ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter( + node, ns + ".collision_detection.min_predicted_path_confidence"); + + // target_type + { + ip.collision_detection.target_type.car = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.car"); + ip.collision_detection.target_type.bus = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.bus"); + ip.collision_detection.target_type.truck = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.truck"); + ip.collision_detection.target_type.trailer = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.trailer"); + ip.collision_detection.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.motorcycle"); + ip.collision_detection.target_type.bicycle = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.bicycle"); + ip.collision_detection.target_type.unknown = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.unknown"); + } + + // velocity_profile + { + ip.collision_detection.velocity_profile.use_upstream = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.use_upstream"); + ip.collision_detection.velocity_profile.minimum_upstream_velocity = + getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.minimum_upstream_velocity"); + ip.collision_detection.velocity_profile.default_velocity = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.default_velocity"); + ip.collision_detection.velocity_profile.minimum_default_velocity = + getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.minimum_default_velocity"); + } + + // fully_prioritized + { + 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"); + } + + // partially_prioritized + { + 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"); + } + + // not_prioritized + { + 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"); + } + + // yield_on_green_traffic_light + { + 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"); + } + + // ignore_on_amber_traffic_light, ignore_on_red_traffic_light + { + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car = + getOrDeclareParameter( + node, + ns + + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car"); + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike = + getOrDeclareParameter( + node, + ns + + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike"); + 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.collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point = + getOrDeclareParameter( + node, ns + + ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_" + "collision_point"); + } + + // occlusion + { + ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + ip.occlusion.occlusion_attention_area_length = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_attention_area_length"); + ip.occlusion.free_space_max = + getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + ip.occlusion.occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + ip.occlusion.denoise_kernel = + getOrDeclareParameter(node, ns + ".occlusion.denoise_kernel"); + 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"); + + // creep_during_peeking + { + ip.occlusion.creep_during_peeking.enable = + getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.enable"); + ip.occlusion.creep_during_peeking.creep_velocity = + getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); + } + + ip.occlusion.peeking_offset = + getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.occlusion_required_clearance_distance = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); + ip.occlusion.possible_object_bbox = + getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); + ip.occlusion.ignore_parked_vehicle_speed_threshold = + getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + ip.occlusion.occlusion_detection_hold_time = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_detection_hold_time"); + ip.occlusion.temporal_stop_time_before_peeking = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_time_before_peeking"); + ip.occlusion.temporal_stop_before_attention_area = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); + ip.occlusion.creep_velocity_without_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.creep_velocity_without_traffic_light"); + ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( + node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); + } + + ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); + + decision_state_pub_ = + node.create_publisher("~/debug/intersection/decision_state", 1); + tl_observation_pub_ = node.create_publisher( + "~/debug/intersection_traffic_signal", 1); +} + +void IntersectionModuleManager::launchNewModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); + + const auto lanelets = + planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); + // run occlusion detection only in the first intersection + for (size_t i = 0; i < lanelets.size(); i++) { + const auto ll = lanelets.at(i); + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + // Is intersection? + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll)) { + continue; + } + + const std::string location = ll.attributeOr("location", "else"); + 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 stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; + } + const auto new_module = std::make_shared( + module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, + has_traffic_light, 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()); + const auto occlusion_uuid = new_module->getOcclusionUUID(); + rtc_interface_.updateCooperateStatus( + uuid, true, State::RUNNING, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, true, State::RUNNING, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + registerModule(std::move(new_module)); + } +} + +std::function &)> +IntersectionModuleManager::getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_set = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [this, lane_set](const std::shared_ptr & scene_module) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = intersection_module->getAssociativeIds(); + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + if (associative_ids.find(lane.id()) != associative_ids.end() /* contains */) { + return false; + } + } + return true; + }; +} + +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane) const +{ + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = intersection_module->getAssociativeIds(); + if (associative_ids.find(lane.id()) != associative_ids.end()) { + return true; + } + } + return false; +} + +void IntersectionModuleManager::sendRTC(const Time & stamp) +{ + double min_distance = std::numeric_limits::infinity(); + std::optional nearest_tl_observation{std::nullopt}; + std_msgs::msg::String decision_type; + + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const UUID uuid = getUUID(scene_module->getModuleId()); + const bool safety = + scene_module->isSafe() && (!intersection_module->isOcclusionFirstStopRequired()); + updateRTCStatus(uuid, safety, State::RUNNING, scene_module->getDistance(), stamp); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + const auto occlusion_distance = intersection_module->getOcclusionDistance(); + const auto occlusion_safety = intersection_module->getOcclusionSafety(); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, occlusion_safety, State::RUNNING, occlusion_distance, occlusion_distance, + stamp); + + // ========================================================================================== + // module debug data + // ========================================================================================== + const auto internal_debug_data = intersection_module->getInternalDebugData(); + if (internal_debug_data.distance < min_distance) { + min_distance = internal_debug_data.distance; + nearest_tl_observation = internal_debug_data.tl_observation; + } + decision_type.data += (internal_debug_data.decision_type + "\n"); + } + rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() + occlusion_rtc_interface_.publishCooperateStatus(stamp); + + // ========================================================================================== + // publish module debug data + // ========================================================================================== + decision_state_pub_->publish(decision_type); + if (nearest_tl_observation) { + tl_observation_pub_->publish(nearest_tl_observation.value().signal); + } +} + +void IntersectionModuleManager::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); + intersection_module->setOcclusionActivation( + occlusion_rtc_interface_.isActivated(occlusion_uuid)); + } +} + +void IntersectionModuleManager::deleteExpiredModules( + const tier4_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)) { + // default + removeRTCStatus(getUUID(scene_module->getModuleId())); + removeUUID(scene_module->getModuleId()); + // occlusion + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + occlusion_rtc_interface_.removeCooperateStatus(occlusion_uuid); + unregisterModule(scene_module); + } + } +} + +MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + auto & mp = merge_from_private_area_param_; + 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.stopline_margin = getOrDeclareParameter(node, ns + ".stopline_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( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); + + const auto lanelets = + planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); + for (size_t i = 0; i < lanelets.size(); i++) { + const auto ll = lanelets.at(i); + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + if (isModuleRegistered(module_id)) { + continue; + } + + // Is intersection? + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + // Is merging from private road? + // In case the goal is in private road, check if this lanelet is conflicting with urban lanelet + const std::string lane_location = ll.attributeOr("location", "else"); + if (lane_location != "private") { + continue; + } + + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll)) { + continue; + } + + if (i + 1 < lanelets.size()) { + const auto next_lane = lanelets.at(i + 1); + const std::string next_lane_location = next_lane.attributeOr("location", "else"); + if (next_lane_location != "private") { + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + registerModule(std::make_shared( + module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, + logger_.get_child("merge_from_private_road_module"), clock_)); + continue; + } + } else { + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, ll); + for (auto && conflicting_lanelet : conflicting_lanelets) { + const std::string conflicting_attr = conflicting_lanelet.attributeOr("location", "else"); + if (conflicting_attr == "urban") { + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + registerModule(std::make_shared( + module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, + logger_.get_child("merge_from_private_road_module"), clock_)); + continue; + } + } + } + } +} + +std::function &)> +MergeFromPrivateModuleManager::getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_set = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [this, lane_set](const std::shared_ptr & scene_module) { + const auto merge_from_private_module = + std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = merge_from_private_module->getAssociativeIds(); + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + if (associative_ids.find(lane.id()) != associative_ids.end() /* contains */) { + return false; + } + } + return true; + }; +} + +bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane) const +{ + for (const auto & scene_module : scene_modules_) { + const auto merge_from_private_module = + std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = merge_from_private_module->getAssociativeIds(); + if (associative_ids.find(lane.id()) != associative_ids.end()) { + return true; + } + } + return false; +} + +} // namespace autoware::behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::IntersectionModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp b/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp new file mode 100644 index 0000000000000..ecf6bb810f313 --- /dev/null +++ b/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp @@ -0,0 +1,95 @@ +// 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene_intersection.hpp" +#include "scene_merge_from_private_road.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC +{ +public: + explicit IntersectionModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "intersection"; } + +private: + IntersectionModule::PlannerParam intersection_param_; + // additional for INTERSECTION_OCCLUSION + RTCInterface occlusion_rtc_interface_; + + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; + + /* called from SceneModuleInterfaceWithRTC::plan */ + void sendRTC(const Time & stamp) override; + void setActivation() override; + /* called from SceneModuleInterface::updateSceneModuleInstances */ + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + rclcpp::Publisher::SharedPtr decision_state_pub_; + rclcpp::Publisher::SharedPtr + tl_observation_pub_; +}; + +class MergeFromPrivateModuleManager : public SceneModuleManagerInterface +{ +public: + explicit MergeFromPrivateModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "merge_from_private"; } + +private: + MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_; + + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; +}; + +class IntersectionModulePlugin : public PluginWrapper +{ +}; + +class MergeFromPrivateModulePlugin : public PluginWrapper +{ +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.cpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/object_manager.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index d2c905673cee9..7473cefa94cac 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -64,7 +64,7 @@ tier4_autoware_utils::Polygon2d createOneStepPolygon( } // namespace -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -201,7 +201,7 @@ std::shared_ptr ObjectInfoManager::registerObject( const bool belong_intersection_area, const bool is_parked_vehicle) { if (objects_info_.count(uuid) == 0) { - auto object = std::make_shared(uuid); + auto object = std::make_shared(uuid); objects_info_[uuid] = object; } auto object = objects_info_[uuid]; @@ -219,7 +219,7 @@ std::shared_ptr ObjectInfoManager::registerObject( void ObjectInfoManager::registerExistingObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, const bool belong_intersection_area, const bool is_parked_vehicle, - std::shared_ptr object) + std::shared_ptr object) { objects_info_[uuid] = object; if (belong_attention_area) { @@ -249,7 +249,7 @@ std::vector> ObjectInfoManager::allObjects() const return all_objects; } -std::optional findPassageInterval( +std::optional findPassageInterval( const autoware_perception_msgs::msg::PredictedPath & predicted_path, const autoware_perception_msgs::msg::Shape & shape, const lanelet::BasicPolygon2d & ego_lane_poly, const std::optional & first_attention_lane_opt, @@ -284,25 +284,25 @@ std::optional findPassageInterval( if (lanelet::geometry::inside( first_attention_lane_opt.value(), lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { - return intersection::CollisionInterval::LanePosition::FIRST; + return CollisionInterval::LanePosition::FIRST; } } if (second_attention_lane_opt) { if (lanelet::geometry::inside( second_attention_lane_opt.value(), lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { - return intersection::CollisionInterval::LanePosition::SECOND; + return CollisionInterval::LanePosition::SECOND; } } - return intersection::CollisionInterval::LanePosition::ELSE; + return CollisionInterval::LanePosition::ELSE; }(); std::vector path; for (const auto & pose : predicted_path.path) { path.push_back(pose); } - return intersection::CollisionInterval{ + return CollisionInterval{ lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp similarity index 96% rename from planning/behavior_velocity_intersection_module/src/object_manager.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp index 180496bd6b18d..0ba9947fdb318 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp @@ -52,7 +52,7 @@ struct hash }; } // namespace std -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -234,8 +234,7 @@ class ObjectInfoManager void registerExistingObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked, - std::shared_ptr object); + const bool belong_intersection_area, const bool is_parked, std::shared_ptr object); void clearObjects(); @@ -282,12 +281,12 @@ class ObjectInfoManager /** * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically */ -std::optional findPassageInterval( +std::optional findPassageInterval( const autoware_perception_msgs::msg::PredictedPath & predicted_path, const autoware_perception_msgs::msg::Shape & shape, const lanelet::BasicPolygon2d & ego_lane_poly, const std::optional & first_attention_lane_opt, const std::optional & second_attention_lane_opt); -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // OBJECT_MANAGER_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/autoware_behavior_velocity_intersection_module/src/result.hpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/result.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/result.hpp index 5d82183cee2fb..1a833b268a5d8 100644 --- a/planning/behavior_velocity_intersection_module/src/result.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/result.hpp @@ -18,7 +18,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { template @@ -48,6 +48,6 @@ Result make_err(Args &&... args) return Result(Error{std::forward(args)...}); } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp similarity index 92% rename from planning/behavior_velocity_intersection_module/src/scene_intersection.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index c27483ea1fa51..beed0aab8e382 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -16,8 +16,8 @@ #include "util.hpp" -#include // for toGeomPoly -#include +#include // for toGeomPoly +#include #include #include #include @@ -38,13 +38,10 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using intersection::make_err; -using intersection::make_ok; -using intersection::Result; using motion_utils::VelocityFactorInterface; IntersectionModule::IntersectionModule( @@ -105,8 +102,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * prev_decision_result_ = decision_result; { - const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + - intersection::formatDecisionResult(decision_result); + const std::string decision_type = + "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); internal_debug_data_.decision_type = decision_type; } @@ -147,7 +144,7 @@ static std::string formatOcclusionType(const IntersectionModule::OcclusionType & return "RTCOccluded"; } -intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( +DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { const auto prepare_data = prepareIntersectionData(path); @@ -187,18 +184,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // ego path has just entered the entry of this intersection // ========================================================================================== if (!intersection_lanelets.first_attention_area()) { - return intersection::InternalError{"attention area is empty"}; + return InternalError{"attention area is empty"}; } const auto first_attention_area = intersection_lanelets.first_attention_area().value(); const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; if (!default_stopline_idx_opt) { - return intersection::InternalError{"default stop line is null"}; + return InternalError{"default stop line is null"}; } const auto default_stopline_idx = default_stopline_idx_opt.value(); const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return intersection::InternalError{"occlusion stop line is null"}; + return InternalError{"occlusion stop line is null"}; } const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); @@ -275,9 +272,9 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto closest_idx = intersection_stoplines.closest_idx; const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); - return intersection::OverPassJudge{safety_diag, evasive_diag}; + return OverPassJudge{safety_diag, evasive_diag}; } - return intersection::OverPassJudge{ + return OverPassJudge{ "no collision is detected", "ego can safely pass the intersection at this rate"}; } @@ -288,8 +285,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // ========================================================================================== std::string safety_report = safety_diag; if (const bool collision_on_1st_attention_lane = - has_collision && - (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + has_collision && (collision_position == CollisionInterval::LanePosition::FIRST); is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line.has_value() && !is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { safety_report += @@ -329,19 +325,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getState() == StateMachine::State::STOP; if (is_prioritized) { - return intersection::FullyPrioritized{ + return FullyPrioritized{ has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report}; } // Safe if (!is_occlusion_state && !has_collision_with_margin) { - return intersection::Safe{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; + return Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // Only collision if (!is_occlusion_state && has_collision_with_margin) { - return intersection::NonOccludedCollisionStop{ + return NonOccludedCollisionStop{ closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // Occluded @@ -400,16 +395,16 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto closest_idx = intersection_stoplines.closest_idx; const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); - return intersection::OverPassJudge{ + return OverPassJudge{ "already passed maximum peeking line in the absence of traffic light.\n" + safety_report, evasive_diag}; } - return intersection::OverPassJudge{ + return OverPassJudge{ "already passed maximum peeking line in the absence of traffic light safely", "no evasive action required"}; } - return intersection::OccludedAbsenceTrafficLight{ + return OccludedAbsenceTrafficLight{ is_occlusion_cleared_with_margin, has_collision_with_margin, temporal_stop_before_attention_required, @@ -439,8 +434,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( 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 intersection::Safe{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; + return Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED const double max_timeout = @@ -453,7 +447,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( occlusion_stop_state_machine_.getDuration()) : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); if (has_collision_with_margin) { - return intersection::OccludedCollisionStop{ + return OccludedCollisionStop{ is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, @@ -463,7 +457,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( static_occlusion_timeout, occlusion_diag}; } else { - return intersection::PeekingTowardOcclusion{ + return PeekingTowardOcclusion{ is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, @@ -478,7 +472,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? first_attention_stopline_idx : occlusion_stopline_idx; - return intersection::FirstWaitBeforeOcclusion{ + return FirstWaitBeforeOcclusion{ is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline, occlusion_diag}; } @@ -505,7 +499,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::InternalError & result, + [[maybe_unused]] const InternalError & result, [[maybe_unused]] const tier4_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) @@ -515,7 +509,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::OverPassJudge & result, + [[maybe_unused]] const OverPassJudge & result, [[maybe_unused]] const tier4_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) @@ -525,7 +519,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::StuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const StuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -545,9 +539,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::YieldStuckStop & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) + const YieldStuckStop & result, const tier4_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; @@ -560,9 +554,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::NonOccludedCollisionStop & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const NonOccludedCollisionStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); const auto closest_idx = result.closest_idx; @@ -579,9 +573,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::FirstWaitBeforeOcclusion & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const FirstWaitBeforeOcclusion & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); const auto closest_idx = result.closest_idx; @@ -598,9 +592,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::PeekingTowardOcclusion & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const PeekingTowardOcclusion & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); const auto closest_idx = result.closest_idx; @@ -617,9 +611,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::OccludedAbsenceTrafficLight & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const OccludedAbsenceTrafficLight & result, const tier4_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; @@ -634,9 +628,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::OccludedCollisionStop & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const OccludedCollisionStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); const auto closest_idx = result.closest_idx; @@ -653,9 +647,8 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); const auto closest_idx = result.closest_idx; @@ -672,9 +665,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::FullyPrioritized & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const FullyPrioritized & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FullyPrioritized"); const auto closest_idx = result.closest_idx; @@ -690,8 +683,7 @@ void prepareRTCByDecisionResult( } void IntersectionModule::prepareRTCStatus( - const intersection::DecisionResult & decision_result, - const tier4_planning_msgs::msg::PathWithLaneId & path) + const DecisionResult & decision_result, const tier4_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; double default_distance = std::numeric_limits::lowest(); @@ -705,7 +697,7 @@ void IntersectionModule::prepareRTCStatus( setSafe(default_safety); setDistance(default_distance); occlusion_first_stop_required_ = - std::holds_alternative(decision_result); + std::holds_alternative(decision_result); } template @@ -723,7 +715,7 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::InternalError & decision_result, + [[maybe_unused]] const InternalError & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, @@ -738,7 +730,7 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::OverPassJudge & decision_result, + [[maybe_unused]] const OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, @@ -752,7 +744,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::StuckStop & decision_result, + const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -799,7 +791,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::YieldStuckStop & decision_result, + const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -832,7 +824,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::NonOccludedCollisionStop & decision_result, + const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -876,7 +868,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::FirstWaitBeforeOcclusion & decision_result, + const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) @@ -927,7 +919,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::PeekingTowardOcclusion & decision_result, + const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) @@ -983,7 +975,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::OccludedCollisionStop & decision_result, + const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1031,7 +1023,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::OccludedAbsenceTrafficLight & decision_result, + const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1084,8 +1076,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( - const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::Safe & decision_result, + const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1128,7 +1119,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::FullyPrioritized & decision_result, + const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1170,8 +1161,8 @@ void reactRTCApprovalByDecisionResult( } void IntersectionModule::reactRTCApproval( - const intersection::DecisionResult & decision_result, - tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) + const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason) { const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( @@ -1293,7 +1284,7 @@ void IntersectionModule::updateTrafficSignalObservation() IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, - const intersection::IntersectionStopLines & intersection_stoplines) + const IntersectionStopLines & intersection_stoplines) { const auto & current_pose = planner_data_->current_odometry->pose; const auto closest_idx = intersection_stoplines.closest_idx; @@ -1346,12 +1337,11 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat // as "was_safe" // ========================================================================================== const bool was_safe = [&]() { - if (std::holds_alternative(prev_decision_result_)) { + if (std::holds_alternative(prev_decision_result_)) { return true; } - if (std::holds_alternative(prev_decision_result_)) { - const auto & state = - std::get(prev_decision_result_); + if (std::holds_alternative(prev_decision_result_)) { + const auto & state = std::get(prev_decision_result_); return !state.collision_detected; } return false; @@ -1422,4 +1412,4 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat safely_passed_1st_judge_line_first_time, safely_passed_2nd_judge_line_first_time}; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp similarity index 89% rename from planning/behavior_velocity_intersection_module/src/scene_intersection.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index b86fd77491f54..e25f41a18028a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -22,8 +22,8 @@ #include "object_manager.hpp" #include "result.hpp" -#include -#include +#include +#include #include #include @@ -43,7 +43,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class IntersectionModule : public SceneModuleInterface @@ -295,11 +295,9 @@ class IntersectionModule : public SceneModuleInterface BLAME_AT_SECOND_PASS_JUDGE, }; const bool collision_detected; - const intersection::CollisionInterval::LanePosition collision_position; - const std::vector>> - too_late_detect_objects; - const std::vector>> - misjudge_objects; + const CollisionInterval::LanePosition collision_position; + const std::vector>> too_late_detect_objects; + const std::vector>> misjudge_objects; }; IntersectionModule( @@ -377,7 +375,7 @@ class IntersectionModule : public SceneModuleInterface */ //! cache IntersectionLanelets struct - std::optional intersection_lanelets_{std::nullopt}; + std::optional intersection_lanelets_{std::nullopt}; //! cache discretized occlusion detection lanelets std::optional> occlusion_attention_divisions_{ @@ -400,7 +398,7 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; //! for checking if ego is over the pass judge lines because previously the situation was SAFE - intersection::DecisionResult prev_decision_result_{intersection::InternalError{""}}; + DecisionResult prev_decision_result_{InternalError{""}}; //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line //! is treated as the same position as occlusion_peeking_stopline @@ -427,7 +425,7 @@ class IntersectionModule : public SceneModuleInterface StateMachine collision_state_machine_; //! container for storing safety status of objects on the attention area - intersection::ObjectInfoManager object_info_manager_; + ObjectInfoManager object_info_manager_; /** @} */ private: @@ -506,21 +504,20 @@ class IntersectionModule : public SceneModuleInterface /** * @brief analyze traffic_light/occupancy/objects context and return DecisionResult */ - intersection::DecisionResult modifyPathVelocityDetail( - PathWithLaneId * path, StopReason * stop_reason); + DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); /** * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const intersection::DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); + const DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); /** * @brief act based on current RTC approval */ void reactRTCApproval( - const intersection::DecisionResult & decision_result, - tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); + const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason); /** @}*/ private: @@ -536,9 +533,9 @@ class IntersectionModule : public SceneModuleInterface */ struct BasicData { - intersection::InterpolatedPathInfo interpolated_path_info; - intersection::IntersectionStopLines intersection_stoplines; - intersection::PathLanelets path_lanelets; + InterpolatedPathInfo interpolated_path_info; + IntersectionStopLines intersection_stoplines; + PathLanelets path_lanelets; }; /** @@ -549,31 +546,30 @@ class IntersectionModule : public SceneModuleInterface * * To simplify modifyPathVelocityDetail(), this function is used at first */ - intersection::Result prepareIntersectionData( - PathWithLaneId * path); + Result prepareIntersectionData(PathWithLaneId * path); /** * @brief find the associated stopline road marking of assigned lanelet */ std::optional getStopLineIndexFromMap( - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) const; /** * @brief generate IntersectionStopLines */ - std::optional generateIntersectionStopLines( + std::optional generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, tier4_planning_msgs::msg::PathWithLaneId * original_path) const; /** * @brief generate IntersectionLanelets */ - intersection::IntersectionLanelets generateObjectiveLanelets( + IntersectionLanelets generateObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet) const; @@ -581,9 +577,9 @@ class IntersectionModule : public SceneModuleInterface /** * @brief generate PathLanelets */ - std::optional generatePathLanelets( + std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, @@ -637,10 +633,9 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_, * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() */ - std::optional isStuckStatus( + std::optional isStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::IntersectionStopLines & intersection_stoplines, - const intersection::PathLanelets & path_lanelets) const; + const IntersectionStopLines & intersection_stoplines, const PathLanelets & path_lanelets) const; bool isTargetStuckVehicleType( const autoware_perception_msgs::msg::PredictedObject & object) const; @@ -651,7 +646,7 @@ class IntersectionModule : public SceneModuleInterface /** * @brief check stuck */ - bool checkStuckVehicleInIntersection(const intersection::PathLanelets & path_lanelets) const; + bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets) const; /** @} */ private: @@ -667,16 +662,16 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_, * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline */ - std::optional isYieldStuckStatus( + std::optional isYieldStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines) const; + const InterpolatedPathInfo & interpolated_path_info, + const IntersectionStopLines & intersection_stoplines) const; /** * @brief check yield stuck */ bool checkYieldStuckVehicleInIntersection( - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const; /** @} */ @@ -698,15 +693,14 @@ class IntersectionModule : public SceneModuleInterface bool /* reconciled occlusion disapproval */> getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info); + const InterpolatedPathInfo & interpolated_path_info); /** * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) * @attention this function has access to value() of intersection_lanelets_, * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_ */ - OcclusionType detectOcclusion( - const intersection::InterpolatedPathInfo & interpolated_path_info) const; + OcclusionType detectOcclusion(const InterpolatedPathInfo & interpolated_path_info) const; /** @} */ private: @@ -726,7 +720,7 @@ class IntersectionModule : public SceneModuleInterface */ PassJudgeStatus isOverPassJudgeLinesStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, - const intersection::IntersectionStopLines & intersection_stoplines); + const IntersectionStopLines & intersection_stoplines); /** @} */ private: @@ -751,7 +745,7 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_ */ void updateObjectInfoManagerCollision( - const intersection::PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, + const PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, const TrafficPrioritizedLevel & traffic_prioritized_level, const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array); @@ -768,20 +762,18 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of * intersection_stoplines.occlusion_peeking_stopline */ - std::optional isGreenPseudoCollisionStatus( + std::optional isGreenPseudoCollisionStatus( const size_t closest_idx, const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines) const; + const IntersectionStopLines & intersection_stoplines) const; /** * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and * blame past perception fault */ std::string generateDetectionBlameDiagnosis( - const std::vector< - std::pair>> & + const std::vector>> & too_late_detect_objects, - const std::vector< - std::pair>> & + const std::vector>> & misjudge_objects) const; /** @@ -791,11 +783,9 @@ class IntersectionModule : public SceneModuleInterface std::string generateEgoRiskEvasiveDiagnosis( const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const TimeDistanceArray & ego_time_distance_array, - const std::vector< - std::pair>> & + const std::vector>> & too_late_detect_objects, - const std::vector< - std::pair>> & + const std::vector>> & misjudge_objects) const; /** @@ -818,7 +808,7 @@ class IntersectionModule : public SceneModuleInterface */ TimeDistanceArray calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, - const intersection::IntersectionStopLines & intersection_stoplines, + const IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const; /** @} */ @@ -828,6 +818,6 @@ class IntersectionModule : public SceneModuleInterface rclcpp::Publisher::SharedPtr object_ttc_pub_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_INTERSECTION_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 131081c5e8ca0..fb06ed9d811c8 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toGeomPoly -#include // for smoothPath +#include // for toGeomPoly +#include // for smoothPath #include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -140,7 +140,7 @@ void IntersectionModule::updateObjectInfoManagerArea() } void IntersectionModule::updateObjectInfoManagerCollision( - const intersection::PathLanelets & path_lanelets, + const PathLanelets & path_lanelets, const IntersectionModule::TimeDistanceArray & time_distance_array, const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, @@ -243,8 +243,8 @@ void IntersectionModule::updateObjectInfoManagerCollision( // path, both will be null, which is interpreted as SAFE. if any of the path is "normal", either // of them has value, not both // ========================================================================================== - std::optional unsafe_interval{std::nullopt}; - std::optional safe_interval{std::nullopt}; + std::optional unsafe_interval{std::nullopt}; + std::optional safe_interval{std::nullopt}; std::optional> object_debug_info{std::nullopt}; for (const auto & predicted_path_ptr : sorted_predicted_paths) { @@ -256,7 +256,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( } cutPredictPathWithinDuration( planner_data_->predicted_objects->header.stamp, passing_time, &predicted_path); - const auto object_passage_interval_opt = intersection::findPassageInterval( + const auto object_passage_interval_opt = findPassageInterval( predicted_path, predicted_object.shape, ego_poly, intersection_lanelets.first_attention_lane(), intersection_lanelets.second_attention_lane()); @@ -362,27 +362,25 @@ void IntersectionModule::updateObjectInfoManagerCollision( } object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control); if (passed_1st_judge_line_first_time) { - object_info->setDecisionAt1stPassJudgeLinePassage(intersection::CollisionKnowledge{ + object_info->setDecisionAt1stPassJudgeLinePassage(CollisionKnowledge{ clock_->now(), // stamp unsafe_interval - ? intersection::CollisionKnowledge::SafeType::UNSAFE - : (safe_under_traffic_control - ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL - : intersection::CollisionKnowledge::SafeType::SAFE), // safe - unsafe_interval ? unsafe_interval : safe_interval, // interval + ? CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control ? CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval predicted_object.kinematics.initial_twist_with_covariance.twist.linear .x // observed_velocity }); } if (passed_2nd_judge_line_first_time) { - object_info->setDecisionAt2ndPassJudgeLinePassage(intersection::CollisionKnowledge{ + object_info->setDecisionAt2ndPassJudgeLinePassage(CollisionKnowledge{ clock_->now(), // stamp unsafe_interval - ? intersection::CollisionKnowledge::SafeType::UNSAFE - : (safe_under_traffic_control - ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL - : intersection::CollisionKnowledge::SafeType::SAFE), // safe - unsafe_interval ? unsafe_interval : safe_interval, // interval + ? CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control ? CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval predicted_object.kinematics.initial_twist_with_covariance.twist.linear .x // observed_velocity }); @@ -415,10 +413,9 @@ void IntersectionModule::cutPredictPathWithinDuration( } } -std::optional -IntersectionModule::isGreenPseudoCollisionStatus( +std::optional IntersectionModule::isGreenPseudoCollisionStatus( const size_t closest_idx, const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines) const + const IntersectionStopLines & intersection_stoplines) const { // ========================================================================================== // if there are any vehicles on the attention area when ego entered the intersection on green @@ -440,7 +437,7 @@ IntersectionModule::isGreenPseudoCollisionStatus( }); if (exist_close_vehicles) { const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); - return intersection::NonOccludedCollisionStop{ + return NonOccludedCollisionStop{ closest_idx, collision_stopline_idx, occlusion_stopline_idx, std::string("")}; } } @@ -448,11 +445,11 @@ IntersectionModule::isGreenPseudoCollisionStatus( } std::string IntersectionModule::generateDetectionBlameDiagnosis( - const std::vector>> & + const std::vector< + std::pair>> & too_late_detect_objects, - const std::vector>> & + const std::vector< + std::pair>> & misjudge_objects) const { std::string diag; @@ -598,11 +595,11 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const IntersectionModule::TimeDistanceArray & ego_time_distance_array, - const std::vector>> & + const std::vector< + std::pair>> & too_late_detect_objects, - [[maybe_unused]] const std::vector>> & + [[maybe_unused]] const std::vector< + std::pair>> & misjudge_objects) const { static constexpr double min_vel = 1e-2; @@ -684,9 +681,8 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( // that case is both "too late to stop" and "too late to go" for the planner. and basically // detection side is responsible for this fault // ========================================================================================== - std::vector>> - misjudge_objects; - std::vector>> + std::vector>> misjudge_objects; + std::vector>> too_late_detect_objects; for (const auto & object_info : object_info_manager_.attentionObjects()) { if (object_info->is_safe_under_traffic_control()) { @@ -709,14 +705,14 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( // visualized as "misjudge" // ========================================================================================== auto * debug_container = &debug_data_.unsafe_targets.objects; - if (unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + if (unsafe_info.lane_position == CollisionInterval::LanePosition::FIRST) { collision_at_first_lane = true; } else { collision_at_non_first_lane = true; } if ( is_over_1st_pass_judge_line && - unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + unsafe_info.lane_position == CollisionInterval::LanePosition::FIRST) { const auto & decision_at_1st_pass_judge_opt = object_info->decision_at_1st_pass_judge_line_passage(); if (!decision_at_1st_pass_judge_opt) { @@ -725,9 +721,7 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( debug_container = &debug_data_.too_late_detect_targets.objects; } else { const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value(); - if ( - decision_at_1st_pass_judge.safe_type != - intersection::CollisionKnowledge::SafeType::UNSAFE) { + if (decision_at_1st_pass_judge.safe_type != CollisionKnowledge::SafeType::UNSAFE) { misjudge_objects.emplace_back( CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); debug_container = &debug_data_.misjudge_targets.objects; @@ -747,9 +741,7 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( debug_container = &debug_data_.too_late_detect_targets.objects; } else { const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value(); - if ( - decision_at_2nd_pass_judge.safe_type != - intersection::CollisionKnowledge::SafeType::UNSAFE) { + if (decision_at_2nd_pass_judge.safe_type != CollisionKnowledge::SafeType::UNSAFE) { misjudge_objects.emplace_back( CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); debug_container = &debug_data_.misjudge_targets.objects; @@ -763,12 +755,11 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( debug_container->emplace_back(object_info->predicted_object()); } if (collision_at_first_lane) { - return { - true, intersection::CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; + return {true, CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; } else if (collision_at_non_first_lane) { - return {true, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + return {true, CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; } - return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + return {false, CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; } std::optional IntersectionModule::checkAngleForTargetLanelets( @@ -811,7 +802,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, - const intersection::IntersectionStopLines & intersection_stoplines, + const IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { const double intersection_velocity = @@ -973,4 +964,4 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin return time_distance_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp similarity index 98% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index b741d43bb025a..c43f8617897b9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -25,7 +25,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -34,7 +34,7 @@ std::tuple< bool /* reconciled occlusion disapproval */> IntersectionModule::getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info) + const InterpolatedPathInfo & interpolated_path_info) { const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); @@ -99,7 +99,7 @@ IntersectionModule::getOcclusionStatus( } IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( - const intersection::InterpolatedPathInfo & interpolated_path_info) const + const InterpolatedPathInfo & interpolated_path_info) const { const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); @@ -449,4 +449,4 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( debug_data_.static_occlusion = true; return StaticallyOccluded{min_dist}; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 9ea5360c3a176..223e8eca84fe8 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for to_bg2d -#include // for planning_utils:: +#include // for to_bg2d +#include // for planning_utils:: #include #include // for lanelet::autoware::RoadMarking #include @@ -106,7 +106,7 @@ std::optional> getFirstPoi 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(behavior_velocity_planner::to_bg2d(p), polygon_2d); + is_in_lanelet = bg::within(autoware::behavior_velocity_planner::to_bg2d(p), polygon_2d); if (is_in_lanelet) { return std::make_optional>( i, polygon); @@ -122,7 +122,7 @@ std::optional> getFirstPoi 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(behavior_velocity_planner::to_bg2d(p), polygon_2d); + is_in_lanelet = bg::within(autoware::behavior_velocity_planner::to_bg2d(p), polygon_2d); if (is_in_lanelet) { return std::make_optional>( i, polygon); @@ -157,16 +157,12 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using intersection::make_err; -using intersection::make_ok; -using intersection::Result; - -Result -IntersectionModule::prepareIntersectionData(PathWithLaneId * path) +Result IntersectionModule::prepareIntersectionData( + PathWithLaneId * path) { const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); @@ -191,13 +187,12 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) 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) { - return make_err( - "splineInterpolate failed"); + return make_err("splineInterpolate failed"); } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - return make_err( + return make_err( "Path has no interval on intersection lane " + std::to_string(lane_id_)); } @@ -231,8 +226,7 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { // this is abnormal - return make_err( - "conflicting area is empty"); + return make_err("conflicting area is empty"); } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); @@ -250,7 +244,7 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, interpolated_path_info, path); if (!intersection_stoplines_opt) { - return make_err( + return make_err( "failed to generate intersection_stoplines"); } const auto & intersection_stoplines = intersection_stoplines_opt.value(); @@ -265,7 +259,7 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { - return make_err( + return make_err( "failed to generate PathLanelets"); } const auto & path_lanelets = path_lanelets_opt.value(); @@ -294,13 +288,12 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) } } - return make_ok( + return make_ok( interpolated_path_info, intersection_stoplines, path_lanelets); } std::optional IntersectionModule::getStopLineIndexFromMap( - const intersection::InterpolatedPathInfo & interpolated_path_info, - lanelet::ConstLanelet assigned_lanelet) const + const InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) const { const auto & path = interpolated_path_info.path; const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); @@ -350,12 +343,11 @@ std::optional IntersectionModule::getStopLineIndexFromMap( planner_data_->ego_nearest_yaw_threshold); } -std::optional -IntersectionModule::generateIntersectionStopLines( +std::optional IntersectionModule::generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, tier4_planning_msgs::msg::PathWithLaneId * original_path) const { const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; @@ -555,7 +547,7 @@ IntersectionModule::generateIntersectionStopLines( intersection_stoplines_temp.default_stopline; } - intersection::IntersectionStopLines intersection_stoplines; + IntersectionStopLines intersection_stoplines; intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; if (stuck_stopline_valid) { intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; @@ -585,7 +577,7 @@ IntersectionModule::generateIntersectionStopLines( return intersection_stoplines; } -intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets( +IntersectionLanelets IntersectionModule::generateObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet) const { @@ -736,7 +728,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets auto [attention_lanelets, original_attention_lanelet_sequences] = util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); - intersection::IntersectionLanelets result; + IntersectionLanelets result; 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 @@ -786,9 +778,9 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets return result; } -std::optional IntersectionModule::generatePathLanelets( +std::optional IntersectionModule::generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, @@ -804,7 +796,7 @@ std::optional IntersectionModule::generatePathLanele const auto assigned_lane_interval = assigned_lane_interval_opt.value(); const auto & path = interpolated_path_info.path; - intersection::PathLanelets path_lanelets; + PathLanelets path_lanelets; // prev path_lanelets.prev = ::getPrevLanelets(lanelets_on_path, associative_ids_); path_lanelets.all = path_lanelets.prev; @@ -915,4 +907,4 @@ std::vector IntersectionModule::generateDetectionLan return detection_divisions; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp similarity index 94% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 498a902c032db..eecb02d07c386 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -15,7 +15,7 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toGeomPoly +#include // for toGeomPoly #include #include @@ -114,14 +114,13 @@ lanelet::ConstLanelet createLaneletFromArcLength( } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -std::optional IntersectionModule::isStuckStatus( +std::optional IntersectionModule::isStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::IntersectionStopLines & intersection_stoplines, - const intersection::PathLanelets & path_lanelets) const + const IntersectionStopLines & intersection_stoplines, const PathLanelets & path_lanelets) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { @@ -162,8 +161,7 @@ std::optional IntersectionModule::isStuckStatus( } } if (stopline_idx) { - return intersection::StuckStop{ - closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; + return StuckStop{closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; } } } @@ -230,8 +228,7 @@ bool IntersectionModule::isTargetYieldStuckVehicleType( return false; } -bool IntersectionModule::checkStuckVehicleInIntersection( - const intersection::PathLanelets & path_lanelets) const +bool IntersectionModule::checkStuckVehicleInIntersection(const PathLanelets & path_lanelets) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; @@ -306,10 +303,10 @@ bool IntersectionModule::checkStuckVehicleInIntersection( return false; } -std::optional IntersectionModule::isYieldStuckStatus( +std::optional IntersectionModule::isYieldStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines) const + const InterpolatedPathInfo & interpolated_path_info, + const IntersectionStopLines & intersection_stoplines) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { @@ -342,14 +339,14 @@ std::optional IntersectionModule::isYieldStuckStat } } if (stopline_idx) { - return intersection::YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; + return YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; } } return std::nullopt; } bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const { const bool yield_stuck_detection_direction = [&]() { @@ -425,4 +422,4 @@ bool IntersectionModule::checkYieldStuckVehicleInIntersection( } return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp similarity index 96% rename from planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 67da3c7a759fe..1aaed779e1b79 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -16,8 +16,8 @@ #include "util.hpp" -#include -#include +#include +#include #include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -52,7 +52,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( static std::optional getFirstConflictingLanelet( const lanelet::ConstLanelets & conflicting_lanelets, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -206,4 +206,4 @@ lanelet::ConstLanelets MergeFromPrivateRoadModule::getAttentionLanelets() const return attention_lanelets; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp similarity index 91% rename from planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index 0b783cf2a7ebd..19e9ea44869ea 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -15,8 +15,8 @@ #ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include -#include +#include +#include #include #include @@ -35,7 +35,7 @@ * lanes before entering intersection */ -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class MergeFromPrivateRoadModule : public SceneModuleInterface { @@ -88,6 +88,6 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ diff --git a/planning/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/autoware_behavior_velocity_intersection_module/src/util.cpp new file mode 100644 index 0000000000000..09f16bcada3c1 --- /dev/null +++ b/planning/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -0,0 +1,402 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "util.hpp" + +#include "interpolated_path_info.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner::util +{ +namespace bg = boost::geometry; + +static std::optional getDuplicatedPointIdx( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) +{ + for (size_t i = 0; i < path.points.size(); i++) { + const auto & p = path.points.at(i).point.pose.position; + + constexpr double min_dist = 0.001; + if (tier4_autoware_utils::calcDistance2d(p, point) < min_dist) { + return i; + } + } + + return std::nullopt; +} + +std::optional insertPointIndex( + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) +{ + const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); + if (duplicate_idx_opt) { + return duplicate_idx_opt.value(); + } + + const size_t closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + inout_path->points, in_pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); + // vector.insert(i) inserts element on the left side of v[i] + // the velocity need to be zero order hold(from prior point) + int insert_idx = closest_idx; + tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); + if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { + ++insert_idx; + } else { + // copy with velocity from prior point + const size_t prior_ind = closest_idx > 0 ? closest_idx - 1 : 0; + inserted_point.point.longitudinal_velocity_mps = + inout_path->points.at(prior_ind).point.longitudinal_velocity_mps; + } + inserted_point.point.pose = in_pose; + + auto it = inout_path->points.begin() + insert_idx; + inout_path->points.insert(it, inserted_point); + + return insert_idx; +} + +bool hasLaneIds( + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids) +{ + for (const auto & pid : p.lane_ids) { + if (ids.find(pid) != ids.end()) { + return true; + } + } + return false; +} + +std::optional> findLaneIdsInterval( + const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) +{ + bool found = false; + size_t start = 0; + size_t end = p.points.size() > 0 ? p.points.size() - 1 : 0; + if (start == end) { + // there is only one point in the path + return std::nullopt; + } + for (size_t i = 0; i < p.points.size(); ++i) { + if (hasLaneIds(p.points.at(i), ids)) { + if (!found) { + // found interval for the first time + found = true; + start = i; + } + } else if (found) { + // prior point was in the interval. interval ended + end = i; + break; + } + } + start = start > 0 ? start - 1 : 0; // the idx of last point before the interval + return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt; +} + +std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + for (auto i = 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; +} + +std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = 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 getFirstPointInsidePolygon( + const tier4_planning_msgs::msg::PathWithLaneId & path, + 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); + 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; +} + +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}; +} + +bool isOverTargetIndex( + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) +{ + if (closest_idx == target_idx) { + const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; + return planning_utils::isAheadOf(current_pose, target_pose); + } + return static_cast(closest_idx > target_idx); +} + +bool isBeforeTargetIndex( + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) +{ + if (closest_idx == target_idx) { + const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; + return planning_utils::isAheadOf(target_pose, current_pose); + } + return static_cast(target_idx > closest_idx); +} + +std::optional getIntersectionArea( + lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) +{ + const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); + if (area_id_str == "else") return std::nullopt; + + const lanelet::Id area_id = std::atoi(area_id_str.c_str()); + const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); + Polygon2d poly{}; + for (const auto & p : poly_3d) poly.outer().emplace_back(p.x(), p.y()); + return std::make_optional(poly); +} + +bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) +{ + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; + } + return tl_id.has_value(); +} + +std::optional generateInterpolatedPath( + const lanelet::Id lane_id, const std::set & associative_lane_ids, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const rclcpp::Logger logger) +{ + InterpolatedPathInfo interpolated_path_info; + if (!splineInterpolate(input_path, ds, interpolated_path_info.path, logger)) { + return std::nullopt; + } + interpolated_path_info.ds = ds; + interpolated_path_info.lane_id = lane_id; + interpolated_path_info.associative_lane_ids = associative_lane_ids; + interpolated_path_info.lane_id_interval = + findLaneIdsInterval(interpolated_path_info.path, associative_lane_ids); + return interpolated_path_info; +} + +geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( + const autoware_perception_msgs::msg::PredictedObjectKinematics & obj_state) +{ + if (obj_state.initial_twist_with_covariance.twist.linear.x >= 0) { + return obj_state.initial_pose_with_covariance.pose; + } + + // When the object velocity is negative, invert orientation (yaw) + auto obj_pose = obj_state.initial_pose_with_covariance.pose; + double yaw, pitch, roll; + tf2::getEulerYPR(obj_pose.orientation, yaw, pitch, roll); + tf2::Quaternion inv_q; + inv_q.setRPY(roll, pitch, yaw + M_PI); + obj_pose.orientation = tf2::toMsg(inv_q); + return obj_pose; +} + +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec) +{ + std::vector polys; + for (auto && ll : ll_vec) { + polys.push_back(ll.polygon3d()); + } + return polys; +} + +} // namespace autoware::behavior_velocity_planner::util diff --git a/planning/autoware_behavior_velocity_intersection_module/src/util.hpp b/planning/autoware_behavior_velocity_intersection_module/src/util.hpp new file mode 100644 index 0000000000000..ef826380afa69 --- /dev/null +++ b/planning/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -0,0 +1,146 @@ +// 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 UTIL_HPP_ +#define UTIL_HPP_ + +#include "interpolated_path_info.hpp" + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner::util +{ + +/** + * @brief insert a new pose to the path and return its index + * @return if insertion was successful return the inserted point index + */ +std::optional insertPointIndex( + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); + +/** + * @brief check if a PathPointWithLaneId contains any of the given lane ids + */ +bool hasLaneIds( + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); + +/** + * @brief find the first contiguous interval of the path points that contains the specified lane ids + * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is + * found, returns the pair (start-1, end) + */ +std::optional> findLaneIdsInterval( + const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); + +/** + * @brief return the index of the first point which is inside the given polygon + * @param[in] lane_interval the interval of the path points on the intersection + * @param[in] search_forward flag for search direction + */ +std::optional getFirstPointInsidePolygon( + const tier4_planning_msgs::msg::PathWithLaneId & path, + 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 + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose + * @return true if ego is over the target_idx + */ +bool isOverTargetIndex( + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); + +/** + * @brief check if ego is before the target_idx. If the index is same, compare the exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose + * @return true if ego is over the target_idx + */ +bool isBeforeTargetIndex( + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); + +std::optional getIntersectionArea( + lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); + +/** + * @brief check if the given lane has related traffic light + */ +bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); + +/** + * @brief interpolate PathWithLaneId + */ +std::optional generateInterpolatedPath( + const lanelet::Id lane_id, const std::set & associative_lane_ids, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const rclcpp::Logger logger); + +geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( + const autoware_perception_msgs::msg::PredictedObjectKinematics & obj_state); + +/** + * @brief this function sorts the set of lanelets topologically using topological sort and merges + * the lanelets from each root to each end. each branch is merged and returned with the original + * lanelets + * @param[in] lanelets the set of lanelets + * @param[in] routing_graph_ptr the routing graph + * @return the pair of merged lanelets and their corresponding original lanelets + */ +std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr); + +/** + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygon + */ +std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + +/** + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygons + */ +std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec); + +} // namespace autoware::behavior_velocity_planner::util + +#endif // UTIL_HPP_ diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt new file mode 100644 index 0000000000000..f4646b1b9eb6e --- /dev/null +++ b/planning/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_no_stopping_area_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/manager.cpp + src/scene_no_stopping_area.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_no_stopping_area_module/README.md b/planning/autoware_behavior_velocity_no_stopping_area_module/README.md similarity index 100% rename from planning/behavior_velocity_no_stopping_area_module/README.md rename to planning/autoware_behavior_velocity_no_stopping_area_module/README.md diff --git a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml b/planning/autoware_behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml similarity index 100% rename from planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml rename to planning/autoware_behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml diff --git a/planning/behavior_velocity_no_stopping_area_module/docs/no-stopping-area.svg b/planning/autoware_behavior_velocity_no_stopping_area_module/docs/no-stopping-area.svg similarity index 100% rename from planning/behavior_velocity_no_stopping_area_module/docs/no-stopping-area.svg rename to planning/autoware_behavior_velocity_no_stopping_area_module/docs/no-stopping-area.svg diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/autoware_behavior_velocity_no_stopping_area_module/package.xml new file mode 100644 index 0000000000000..1a2d435fb6a4a --- /dev/null +++ b/planning/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -0,0 +1,46 @@ + + + + autoware_behavior_velocity_no_stopping_area_module + 0.1.0 + The autoware_behavior_velocity_no_stopping_area_module package + + Kosuke Takeuchi + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Kosuke Takeuchi + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_behavior_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_vehicle_info_utils + eigen + geometry_msgs + interpolation + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + tf2 + tf2_eigen + tf2_geometry_msgs + tier4_autoware_utils + tier4_planning_msgs + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/plugins.xml b/planning/autoware_behavior_velocity_no_stopping_area_module/plugins.xml new file mode 100644 index 0000000000000..54bc5584007c2 --- /dev/null +++ b/planning/autoware_behavior_velocity_no_stopping_area_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp new file mode 100644 index 0000000000000..e90273d4ac5e4 --- /dev/null +++ b/planning/autoware_behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -0,0 +1,178 @@ +// Copyright 2021 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_no_stopping_area.hpp" + +#include +#include +#include +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +namespace +{ +const double marker_lifetime = 0.2; +using DebugData = NoStoppingAreaModule::DebugData; +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; + +lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) +{ + lanelet::BasicPoint3d p_sum{0.0, 0.0, 0.0}; + for (const auto & p : poly) { + p_sum += p; + } + return p_sum / poly.size(); +} + +geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) +{ + geometry_msgs::msg::Point msg; + msg.x = point.x(); + msg.y = point.y(); + msg.z = point.z(); + return msg; +} + +visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const rclcpp::Time & now) +{ + visualization_msgs::msg::MarkerArray msg; + + // ID + { + auto marker = createDefaultMarker( + "map", now, "no_stopping_area_id", no_stopping_area_reg_elem.id(), + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), + createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); + + for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { + const auto poly = detection_area.basicPolygon(); + + marker.pose.position = toMsg(poly.front()); + marker.pose.position.z += 2.0; + marker.text = std::to_string(no_stopping_area_reg_elem.id()); + + msg.markers.push_back(marker); + } + } + + // Polygon + { + auto marker = createDefaultMarker( + "map", now, "no_stopping_area_polygon", no_stopping_area_reg_elem.id(), + visualization_msgs::msg::Marker::LINE_LIST, createMarkerScale(0.1, 0.0, 0.0), + createMarkerColor(0.1, 0.1, 1.0, 0.500)); + marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); + + for (const auto & no_stopping_area : no_stopping_area_reg_elem.noStoppingAreas()) { + const auto poly = no_stopping_area.basicPolygon(); + + for (size_t i = 0; i < poly.size(); ++i) { + const auto idx_front = i; + const auto idx_back = (i == poly.size() - 1) ? 0 : i + 1; + + const auto & p_front = poly.at(idx_front); + const auto & p_back = poly.at(idx_back); + + marker.points.push_back(toMsg(p_front)); + marker.points.push_back(toMsg(p_back)); + } + } + msg.markers.push_back(marker); + } + + const auto & stop_line = no_stopping_area_reg_elem.stopLine(); + // Polygon to StopLine + if (stop_line) { + const auto stop_line_center_point = + (stop_line.value().front().basicPoint() + stop_line.value().back().basicPoint()) / 2; + auto marker = createDefaultMarker( + "map", now, "no_stopping_area_correspondence", no_stopping_area_reg_elem.id(), + visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + createMarkerColor(0.1, 0.1, 1.0, 0.500)); + marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); + for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { + const auto poly = detection_area.basicPolygon(); + const auto centroid_point = getCentroidPoint(poly); + for (size_t i = 0; i < poly.size(); ++i) { + marker.points.push_back(toMsg(centroid_point)); + marker.points.push_back(toMsg(stop_line_center_point)); + } + } + msg.markers.push_back(marker); + } + + return msg; +} +} // namespace + +visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + const rclcpp::Time now = clock_->now(); + + appendMarkerArray( + createLaneletInfoMarkerArray(no_stopping_area_reg_elem_, now), &debug_marker_array, now); + + if (!debug_data_.stuck_points.empty()) { + appendMarkerArray( + debug::createPointsMarkerArray( + debug_data_.stuck_points, "stuck_points", module_id_, now, 0.3, 0.3, 0.3, 1.0, 1.0, 0.0), + &debug_marker_array, now); + } + if (!debug_data_.stuck_vehicle_detect_area.points.empty()) { + appendMarkerArray( + debug::createPolygonMarkerArray( + debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", module_id_, now, 0.1, + 0.1, 0.1, 1.0, 1.0, 0.0), + &debug_marker_array, now); + } + if (!debug_data_.stop_line_detect_area.points.empty()) { + appendMarkerArray( + debug::createPolygonMarkerArray( + debug_data_.stop_line_detect_area, "stop_line_detect_area", module_id_, now, 0.1, 0.1, 0.1, + 1.0, 1.0, 0.0), + &debug_marker_array, now); + } + return debug_marker_array; +} + +motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; + wall.ns = std::to_string(module_id_) + "_"; + wall.text = "no_stopping_area"; + wall.style = motion_utils::VirtualWallType::stop; + for (const auto & p : debug_data_.stop_poses) { + wall.pose = tier4_autoware_utils::calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + virtual_walls.push_back(wall); + } + return virtual_walls; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp new file mode 100644 index 0000000000000..31682fe0419d2 --- /dev/null +++ b/planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -0,0 +1,94 @@ +// Copyright 2021 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 +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using lanelet::autoware::NoStoppingArea; +using tier4_autoware_utils::getOrDeclareParameter; + +NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) +{ + const std::string ns(getModuleName()); + auto & pp = planner_param_; + const auto & vi = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); + 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 = + getOrDeclareParameter(node, ns + ".stuck_vehicle_front_margin"); + pp.path_expand_width = vi.vehicle_width_m * 0.5; +} + +void NoStoppingAreaModuleManager::launchNewModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & m : planning_utils::getRegElemMapOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), + planner_data_->current_odometry->pose)) { + // Use lanelet_id to unregister module when the route is changed + const int64_t module_id = m.first->id(); + const int64_t lane_id = m.second.id(); + + if (!isModuleRegistered(module_id)) { + // assign 1 no stopping area for each module + registerModule(std::make_shared( + module_id, lane_id, *m.first, planner_param_, logger_.get_child("no_stopping_area_module"), + clock_)); + generateUUID(module_id); + updateRTCStatus( + getUUID(module_id), true, State::WAITING_FOR_EXECUTION, + std::numeric_limits::lowest(), path.header.stamp); + } + } +} + +std::function &)> +NoStoppingAreaModuleManager::getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [no_stopping_area_id_set](const std::shared_ptr & scene_module) { + return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; + }; +} + +} // namespace autoware::behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp new file mode 100644 index 0000000000000..696115d4bda6d --- /dev/null +++ b/planning/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 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_no_stopping_area.hpp" + +#include +#include +#include +#include + +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC +{ +public: + explicit NoStoppingAreaModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "no_stopping_area"; } + +private: + NoStoppingAreaModule::PlannerParam planner_param_; + + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class NoStoppingAreaModulePlugin : public PluginWrapper +{ +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp similarity index 98% rename from planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp rename to planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 3f2e7581a4d14..a89161de431ae 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -14,9 +14,9 @@ #include "scene_no_stopping_area.hpp" -#include -#include -#include +#include +#include +#include #include #include #include @@ -36,7 +36,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -416,4 +416,4 @@ void NoStoppingAreaModule::insertStopPoint( // Insert stop point or replace with zero velocity planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp similarity index 94% rename from planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp rename to planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 62ec0b88b328e..e7e66775f368e 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -18,9 +18,9 @@ #define EIGEN_MPL2_ONLY #include -#include -#include -#include +#include +#include +#include #include #include @@ -38,7 +38,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d @@ -172,6 +172,6 @@ class NoStoppingAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_NO_STOPPING_AREA_HPP_ diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt new file mode 100644 index 0000000000000..4e53e7bd1790b --- /dev/null +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_occlusion_spot_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/debug.cpp + src/grid_utils.cpp + src/manager.cpp + src/occlusion_spot_utils.cpp + src/risk_predictive_braking.cpp + src/scene_occlusion_spot.cpp +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_occlusion_spot_utils.cpp + test/src/test_risk_predictive_braking.cpp + test/src/test_grid_utils.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) + target_include_directories(test_${PROJECT_NAME} PRIVATE src) +endif() + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_occlusion_spot_module/README.md b/planning/autoware_behavior_velocity_occlusion_spot_module/README.md similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/README.md rename to planning/autoware_behavior_velocity_occlusion_spot_module/README.md diff --git a/planning/behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml b/planning/autoware_behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml rename to planning/autoware_behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/da.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/da.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/da.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/da.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml new file mode 100644 index 0000000000000..e229a69032c27 --- /dev/null +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -0,0 +1,47 @@ + + + + autoware_behavior_velocity_occlusion_spot_module + 0.1.0 + The autoware_behavior_velocity_occlusion_spot_module package + + Taiki Tanaka + Tomoya Kimura + Shumpei Wakabayashi + + Apache License 2.0 + + Taiki Tanaka + + ament_cmake_auto + autoware_cmake + + autoware_behavior_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs + autoware_route_handler + autoware_vehicle_info_utils + geometry_msgs + grid_map_ros + grid_map_utils + interpolation + lanelet2_extension + libboost-dev + libopencv-dev + motion_utils + nav_msgs + pluginlib + rclcpp + tf2 + tf2_geometry_msgs + tier4_autoware_utils + visualization_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml b/planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml new file mode 100644 index 0000000000000..53a2c460c2e44 --- /dev/null +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp new file mode 100644 index 0000000000000..40e5b8f91e6b9 --- /dev/null +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -0,0 +1,230 @@ +// Copyright 2021 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 "occlusion_spot_utils.hpp" +#include "scene_occlusion_spot.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +namespace +{ +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::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerPosition; +using tier4_autoware_utils::createMarkerScale; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +std::vector makeDebugInfoMarker( + const PossibleCollisionInfo & possible_collision, const int id, const bool show_text) +{ + std::vector debug_markers; + Marker debug_marker; + debug_marker.header.frame_id = "map"; + debug_marker.id = id; + debug_marker.action = Marker::ADD; + debug_marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); + debug_marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5); + const auto & pc = possible_collision; + // for collision point with margin + { + debug_marker.ns = "collision_point"; + debug_marker.type = Marker::CYLINDER; + debug_marker.pose = pc.collision_with_margin.pose; + debug_marker.scale = createMarkerScale(0.5, 0.5, 0.5); + debug_marker.color = createMarkerColor(1.0, 0.0, 0.0, 0.5); + debug_markers.push_back(debug_marker); + } + // cylinder at collision_point point + { + debug_marker.ns = "collision_point_with_margin"; + debug_marker.type = Marker::CYLINDER; + debug_marker.pose = pc.collision_pose; + debug_marker.scale = createMarkerScale(0.5, 0.5, 0.5); + debug_marker.color = createMarkerColor(1.0, 0.5, 0.0, 0.5); + debug_markers.push_back(debug_marker); + } + + // cylinder at obstacle point + { + debug_marker.ns = "obstacle"; + debug_marker.type = Marker::CYLINDER; + debug_marker.pose.position = pc.obstacle_info.position; + debug_marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.5); + debug_marker.scale = createMarkerScale(0.8, 0.8, 1.5); + debug_markers.push_back(debug_marker); + } + + // arrow marker + { + debug_marker.ns = "from_obj_to_collision"; + debug_marker.type = Marker::ARROW; + debug_marker.scale = createMarkerScale(0.05, 0.2, 0.5); + debug_marker.color = createMarkerColor(0.1, 0.1, 0.1, 0.5); + debug_marker.points = {pc.obstacle_info.position, pc.intersection_pose.position}; + debug_markers.push_back(debug_marker); + } + + if (show_text) { + // info text at obstacle point + debug_marker.ns = "info"; + debug_marker.type = Marker::TEXT_VIEW_FACING; + debug_marker.pose = pc.collision_with_margin.pose; + debug_marker.scale.z = 1.0; + debug_marker.color = createMarkerColor(1.0, 1.0, 0.0, 1.0); + std::ostringstream string_stream; + auto r = [](const double v) { return std::round(v * 100.0) / 100.0; }; + const double len = r(pc.arc_lane_dist_at_collision.length); + const double dist = r(pc.arc_lane_dist_at_collision.distance); + const double vel = r(pc.obstacle_info.safe_motion.safe_velocity); + const double margin = r(pc.obstacle_info.safe_motion.stop_dist); + string_stream << "(s,d,v,m)=(" << len << " , " << dist << " , " << vel << " , " << margin + << " )"; + debug_marker.text = string_stream.str(); + debug_markers.push_back(debug_marker); + } + return debug_markers; +} + +template +MarkerArray makeDebugInfoMarkers(T & debug_data) +{ + // add slow down markers for occlusion spot + MarkerArray debug_markers; + const auto & possible_collisions = debug_data.possible_collisions; + size_t id = 0; + // draw obstacle collision + for (const auto & pc : possible_collisions) { + // debug marker + std::vector collision_markers = makeDebugInfoMarker(pc, id, true); + debug_markers.markers.insert( + debug_markers.markers.end(), collision_markers.begin(), collision_markers.end()); + id++; + } + return debug_markers; +} + +MarkerArray makePolygonMarker( + const BasicPolygons & polygons, const std::string ns, const int id, const double z) +{ + MarkerArray debug_markers; + Marker debug_marker; + debug_marker.header.frame_id = "map"; + debug_marker.header.stamp = rclcpp::Time(0); + debug_marker.id = planning_utils::bitShift(id); + debug_marker.type = Marker::LINE_STRIP; + debug_marker.action = Marker::ADD; + debug_marker.pose.position = createMarkerPosition(0.0, 0.0, 0); + debug_marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + debug_marker.scale = createMarkerScale(0.1, 0.1, 0.1); + debug_marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.5); + debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5); + debug_marker.ns = ns; + for (const auto & poly : polygons) { + for (const auto & p : poly) { + geometry_msgs::msg::Point point = createMarkerPosition(p.x(), p.y(), z + 0.5); + debug_marker.points.push_back(point); + } + debug_markers.markers.push_back(debug_marker); + debug_marker.id++; + debug_marker.points.clear(); + } + return debug_markers; +} + +MarkerArray makeSlicePolygonMarker( + const Polygons2d & slices, const std::string ns, const int id, const double z) +{ + MarkerArray debug_markers; + Marker debug_marker; + debug_marker.header.frame_id = "map"; + debug_marker.header.stamp = rclcpp::Time(0); + debug_marker.id = planning_utils::bitShift(id); + debug_marker.type = Marker::LINE_STRIP; + debug_marker.action = Marker::ADD; + debug_marker.pose.position = createMarkerPosition(0.0, 0.0, 0); + debug_marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + debug_marker.scale = createMarkerScale(0.1, 0.1, 0.1); + debug_marker.color = createMarkerColor(1.0, 0.0, 1.0, 0.3); + debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); + debug_marker.ns = ns; + for (const auto & slice : slices) { + for (const auto & p : slice.outer()) { + geometry_msgs::msg::Point point = createMarkerPosition(p.x(), p.y(), z); + debug_marker.points.push_back(point); + } + debug_markers.markers.push_back(debug_marker); + debug_marker.id++; + debug_marker.points.clear(); + } + return debug_markers; +} +} // namespace + +MarkerArray OcclusionSpotModule::createDebugMarkerArray() +{ + const auto now = this->clock_->now(); + MarkerArray debug_marker_array; + if (!debug_data_.possible_collisions.empty()) { + appendMarkerArray(makeDebugInfoMarkers(debug_data_), &debug_marker_array, now); + } + if (!debug_data_.detection_area_polygons.empty()) { + appendMarkerArray( + makeSlicePolygonMarker( + debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z), + &debug_marker_array, now); + } + if (!debug_data_.close_partition.empty() && param_.is_show_occlusion) { + appendMarkerArray( + makePolygonMarker(debug_data_.close_partition, "close_partition", module_id_, debug_data_.z), + &debug_marker_array, now); + } + if (!debug_data_.occlusion_points.empty()) { + appendMarkerArray( + debug::createPointsMarkerArray( + debug_data_.occlusion_points, "occlusion", module_id_, now, 0.5, 0.5, 0.5, 1.0, 0.0, 0.0), + &debug_marker_array, now); + } + return debug_marker_array; +} + +motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() +{ + motion_utils::VirtualWalls virtual_walls; + motion_utils::VirtualWall wall; + wall.text = "occlusion_spot"; + wall.style = motion_utils::VirtualWallType::slowdown; + for (size_t id = 0; id < debug_data_.debug_poses.size(); id++) { + wall.pose = + calcOffsetPose(debug_data_.debug_poses.at(id), debug_data_.baselink_to_front, 0.0, 0.0); + virtual_walls.push_back(wall); + } + return virtual_walls; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp similarity index 98% rename from planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index c8adb324b9055..6ab88baf77b20 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -20,7 +20,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace grid_utils { @@ -39,7 +39,7 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) // std::cout << boost::geometry::wkt(line_poly) << std::endl; // std::cout << boost::geometry::wkt(line) << std::endl; - bg::correct(line_poly); + boost::geometry::correct(line_poly); return line_poly; } @@ -142,7 +142,7 @@ Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, cons poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, r, 0).position)); poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, r, 0).position)); - bg::correct(poly); + boost::geometry::correct(poly); return poly; } @@ -381,4 +381,4 @@ void denoiseOccupancyGridCV( grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); } } // namespace grid_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp similarity index 92% rename from planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 710f671df609f..8dc59d07173dc 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -15,8 +15,8 @@ #ifndef GRID_UTILS_HPP_ #define GRID_UTILS_HPP_ -#include -#include +#include +#include #include #include #include @@ -49,7 +49,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -61,6 +61,10 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; using nav_msgs::msg::OccupancyGrid; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + namespace occlusion_cost_value { static constexpr unsigned char FREE_SPACE = 0; @@ -118,6 +122,6 @@ void denoiseOccupancyGridCV( grid_map::GridMap & grid_map, const GridParam & param, const bool is_show_debug_window, const int num_iter, const bool use_object_footprints, const bool use_object_ray_casts); } // namespace grid_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // GRID_UTILS_HPP_ diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp new file mode 100644 index 0000000000000..c1d345cbc9aac --- /dev/null +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -0,0 +1,144 @@ +// Copyright 2021 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 "scene_occlusion_spot.hpp" + +#include +#include + +#include + +#include +#include +#include + +namespace autoware::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()) +{ + const std::string ns(getModuleName()); + auto & pp = planner_param_; + // for detection type + { + 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; + } else if (method == "predicted_object") { // module id 1 + pp.detection_method = DETECTION_METHOD::PREDICTED_OBJECT; + module_id_ = DETECTION_METHOD::PREDICTED_OBJECT; + } else { + throw std::invalid_argument{ + "[behavior_velocity]: occlusion spot detection method has invalid argument"}; + } + } + // for passable judgement + { + 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") { + pp.pass_judge = PASS_JUDGE::SMOOTH_VELOCITY; + } else { + throw std::invalid_argument{ + "[behavior_velocity]: occlusion spot pass judge method has invalid argument"}; + } + } + 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 = 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 = + getOrDeclareParameter(node, ns + ".threshold.detection_area_offset"); + pp.detection_area_length = + 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 = 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 = + 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 = + getOrDeclareParameter(node, ns + ".detection_area.min_occlusion_spot_size"); + pp.detection_area.min_longitudinal_offset = + getOrDeclareParameter(node, ns + ".detection_area.min_longitudinal_offset"); + pp.detection_area.max_lateral_distance = + getOrDeclareParameter(node, ns + ".detection_area.max_lateral_distance"); + pp.detection_area.slice_length = + getOrDeclareParameter(node, ns + ".detection_area.slice_length"); + // occupancy grid param + 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 = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); + pp.baselink_to_front = vehicle_info.max_longitudinal_offset_m; + pp.wheel_tread = vehicle_info.wheel_tread_m; + pp.right_overhang = vehicle_info.right_overhang_m; + pp.left_overhang = vehicle_info.left_overhang_m; +} + +void OcclusionSpotModuleManager::launchNewModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + if (path.points.empty()) return; + // general + if (!isModuleRegistered(module_id_)) { + registerModule(std::make_shared( + module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), + clock_)); + } +} + +std::function &)> +OcclusionSpotModuleManager::getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { + return false; + }; +} +} // namespace autoware::behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::OcclusionSpotModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp new file mode 100644 index 0000000000000..123d0ef32afc1 --- /dev/null +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -0,0 +1,68 @@ +// Copyright 2021 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 "occlusion_spot_utils.hpp" +#include "scene_occlusion_spot.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +class OcclusionSpotModuleManager : public SceneModuleManagerInterface +{ +public: + explicit OcclusionSpotModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "occlusion_spot"; } + +private: + enum class ModuleID { OCCUPANCY, OBJECT }; + using PlannerParam = occlusion_spot_utils::PlannerParam; + + PlannerParam planner_param_; + int64_t module_id_; + + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class OcclusionSpotModulePlugin : public PluginWrapper +{ +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp similarity index 98% rename from planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index a650a6fa5b39f..ed195023ce67a 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -16,8 +16,8 @@ #include "risk_predictive_braking.hpp" -#include -#include +#include +#include #include #include #include @@ -33,7 +33,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; namespace occlusion_spot_utils @@ -485,4 +485,4 @@ std::optional generateOneNotableCollisionFromOcclusionSpo } } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp similarity index 98% rename from planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 2b6d89680cd37..542c869919339 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -17,7 +17,7 @@ #include "grid_utils.hpp" -#include +#include #include #include #include @@ -46,7 +46,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; @@ -246,6 +246,6 @@ bool generatePossibleCollisionsFromGridMap( DebugData & debug_data); } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // OCCLUSION_SPOT_UTILS_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp similarity index 95% rename from planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp index 96f497ca47afa..9fe06273ea617 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp @@ -16,12 +16,12 @@ #include "occlusion_spot_utils.hpp" -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace occlusion_spot_utils { @@ -98,4 +98,4 @@ SafeMotion calculateSafeMotion(const Velocity & v, const double ttv) return sm; } } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp similarity index 93% rename from planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp index 8d7e9d2fdedd5..9def3b0938998 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp @@ -22,7 +22,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace occlusion_spot_utils { @@ -38,6 +38,6 @@ void applySafeVelocityConsideringPossibleCollision( SafeMotion calculateSafeMotion(const Velocity & v, const double ttv); } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // RISK_PREDICTIVE_BRAKING_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp similarity index 94% rename from planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 1f5ca1bab904e..e5c5eaec346ff 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -17,10 +17,10 @@ #include "occlusion_spot_utils.hpp" #include "risk_predictive_braking.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -41,7 +41,7 @@ namespace { -namespace utils = behavior_velocity_planner::occlusion_spot_utils; +namespace utils = autoware::behavior_velocity_planner::occlusion_spot_utils; using autoware_perception_msgs::msg::PredictedObject; std::vector extractStuckVehicle( const std::vector & vehicles, const double stop_velocity) @@ -56,7 +56,7 @@ std::vector extractStuckVehicle( } } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace utils = occlusion_spot_utils; @@ -201,4 +201,4 @@ bool OcclusionSpotModule::modifyPathVelocity( return true; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp similarity index 89% rename from planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index b7da7c073cbd9..00126a2ebc39a 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -17,8 +17,8 @@ #include "occlusion_spot_utils.hpp" -#include -#include +#include +#include #include #include @@ -37,7 +37,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class OcclusionSpotModule : public SceneModuleInterface { @@ -70,6 +70,6 @@ class OcclusionSpotModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_OCCLUSION_SPOT_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp similarity index 93% rename from planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index bc526959386e9..a21e0333082a9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -15,7 +15,7 @@ #include "grid_utils.hpp" #include "utils.hpp" -#include +#include #include #include @@ -38,11 +38,11 @@ struct indexEq } }; -using behavior_velocity_planner::LineString2d; -using behavior_velocity_planner::Point2d; -using behavior_velocity_planner::Polygon2d; -using behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; -using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; +using autoware::behavior_velocity_planner::LineString2d; +using autoware::behavior_velocity_planner::Point2d; +using autoware::behavior_velocity_planner::Polygon2d; +using autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; +using autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; namespace bg = boost::geometry; Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp similarity index 87% rename from planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 05e73855f2642..60ef5d62a5b7b 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -16,8 +16,8 @@ #include "occlusion_spot_utils.hpp" #include "utils.hpp" -#include -#include +#include +#include #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/vector3.hpp" @@ -32,8 +32,9 @@ using tier4_planning_msgs::msg::PathWithLaneId; TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) { - using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils:: + calcSlowDownPointsForPossibleCollision; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::high_resolution_clock; @@ -65,8 +66,9 @@ TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) TEST(calcSlowDownPointsForPossibleCollision, ConsiderSignedOffset) { - using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils:: + calcSlowDownPointsForPossibleCollision; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::high_resolution_clock; diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp similarity index 95% rename from planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp index 9b1d92b71a111..4a0071be10b2d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp @@ -23,7 +23,7 @@ TEST(safeMotion, delay_jerk_acceleration) { - namespace utils = behavior_velocity_planner::occlusion_spot_utils; + namespace utils = autoware::behavior_velocity_planner::occlusion_spot_utils; using utils::calculateSafeMotion; /** * @brief check if calculation is correct in below parameter diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp new file mode 100644 index 0000000000000..4b8a7ae3b2f3f --- /dev/null +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -0,0 +1,107 @@ +// Copyright 2021 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include "occlusion_spot_utils.hpp" + +#include + +#include +#include + +#include + +namespace test +{ + +inline tier4_planning_msgs::msg::PathWithLaneId generatePath( + double x0, double y0, double x, double y, int nb_points) +{ + tier4_planning_msgs::msg::PathWithLaneId path{}; + double x_step = (x - x0) / (nb_points - 1); + double y_step = (y - y0) / (nb_points - 1); + for (int i = 0; i < nb_points; ++i) { + tier4_planning_msgs::msg::PathPointWithLaneId point{}; + point.point.pose.position.x = x0 + x_step * i; + point.point.pose.position.y = y0 + y_step * i; + point.point.pose.position.z = 0.0; + path.points.push_back(point); + } + return path; +} + +// /!\ columns and rows in the GridMap are "inverted" (x -> rows, y -> columns) +inline grid_map::GridMap generateGrid(int w, int h, double res) +{ + grid_map::GridMap grid{}; + grid_map::Length length(w * res, h * res); + grid.setGeometry(length, res, grid_map::Position(length.x() / 2.0, length.y() / 2.0)); + grid.add( + "layer", autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); + return grid; +} + +using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; +inline void generatePossibleCollisions( + std::vector & possible_collisions, double x0, double y0, double x, + double y, int nb_cols) +{ + using autoware::behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + const double lon = 0.0; // assume col_x = intersection_x + const double lat = -1.0; + const double velocity = 1.0; + /** + * @brief representation of a possible collision between ego and some obstacle + * ^ + * | + * Ego ---------collision----------intersection-------> path + * | + * ------------------ | + * | Vehicle | obstacle + * ------------------ + */ + double x_step = (x - x0) / (nb_cols - 1); + double y_step = (y - y0) / (nb_cols - 1); + for (int i = 0; i < nb_cols; ++i) { + // collision + ObstacleInfo obstacle_info; + obstacle_info.position.x = x0 + x_step * i; + obstacle_info.position.y = y0 + y_step * i + lat; + obstacle_info.max_velocity = velocity; + + // intersection + geometry_msgs::msg::Pose intersection_pose{}; + intersection_pose.position.x = x0 + x_step * i + lon; + intersection_pose.position.x = y0 + y_step * i; + + // collision path point + autoware_planning_msgs::msg::PathPoint collision_with_margin{}; + collision_with_margin.pose.position.x = x0 + x_step * i + lon; + collision_with_margin.pose.position.y = y0 + y_step * i; + + lanelet::ArcCoordinates arc; + arc.length = obstacle_info.position.x; + arc.distance = obstacle_info.position.y; + + PossibleCollisionInfo col(obstacle_info, collision_with_margin, intersection_pose, arc); + possible_collisions.emplace_back(col); + } +} + +} // namespace test + +#endif // UTILS_HPP_ diff --git a/planning/autoware_behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md index 0bbc2fad3c5a8..5b6ec1f6fa0e5 100644 --- a/planning/autoware_behavior_velocity_planner/README.md +++ b/planning/autoware_behavior_velocity_planner/README.md @@ -7,18 +7,18 @@ It loads modules as plugins. Please refer to the links listed below for detail o ![Architecture](./docs/BehaviorVelocityPlanner-Architecture.drawio.svg) -- [Blind Spot](../behavior_velocity_blind_spot_module/README.md) -- [Crosswalk](../behavior_velocity_crosswalk_module/README.md) -- [Walkway](../behavior_velocity_walkway_module/README.md) +- [Blind Spot](../autoware_behavior_velocity_blind_spot_module/README.md) +- [Crosswalk](../autoware_behavior_velocity_crosswalk_module/README.md) +- [Walkway](../autoware_behavior_velocity_walkway_module/README.md) - [Detection Area](../behavior_velocity_detection_area_module/README.md) -- [Intersection](../behavior_velocity_intersection_module/README.md) +- [Intersection](../autoware_behavior_velocity_intersection_module/README.md) - [MergeFromPrivate](../behavior_velocity_intersection_module/README.md#merge-from-private) -- [Stop Line](../behavior_velocity_stop_line_module/README.md) +- [Stop Line](../autoware_behavior_velocity_stop_line_module/README.md) - [Virtual Traffic Light](../autoware_behavior_velocity_virtual_traffic_light_module/README.md) -- [Traffic Light](../behavior_velocity_traffic_light_module/README.md) -- [Occlusion Spot](../behavior_velocity_occlusion_spot_module/README.md) +- [Traffic Light](../autoware_behavior_velocity_traffic_light_module/README.md) +- [Occlusion Spot](../autoware_behavior_velocity_occlusion_spot_module/README.md) - [No Stopping Area](../behavior_velocity_no_stopping_area_module/README.md) -- [Run Out](../behavior_velocity_run_out_module/README.md) +- [Run Out](../autoware_behavior_velocity_run_out_module/README.md) - [Speed Bump](../behavior_velocity_speed_bump_module/README.md) - [Out of Lane](../behavior_velocity_out_of_lane_module/README.md) diff --git a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index d4478f87f0610..f52d9637c2134 100644 --- a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -24,7 +24,6 @@ - @@ -68,7 +67,6 @@ - diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index 3d51c6b855eb7..dde7ab91d70b2 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -34,11 +34,12 @@ rosidl_default_generators + autoware_behavior_velocity_planner_common autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_route_handler autoware_velocity_smoother - behavior_velocity_planner_common diagnostic_msgs eigen geometry_msgs @@ -49,7 +50,6 @@ pluginlib rclcpp rclcpp_components - route_handler sensor_msgs tf2 tf2_eigen @@ -65,21 +65,21 @@ ament_cmake_ros ament_lint_auto + autoware_behavior_velocity_blind_spot_module + autoware_behavior_velocity_blind_spot_module + autoware_behavior_velocity_crosswalk_module + autoware_behavior_velocity_intersection_module + autoware_behavior_velocity_no_stopping_area_module + autoware_behavior_velocity_occlusion_spot_module + autoware_behavior_velocity_run_out_module + autoware_behavior_velocity_stop_line_module + autoware_behavior_velocity_traffic_light_module autoware_behavior_velocity_virtual_traffic_light_module + autoware_behavior_velocity_walkway_module autoware_lint_common - behavior_velocity_blind_spot_module - behavior_velocity_crosswalk_module behavior_velocity_detection_area_module - behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module - behavior_velocity_no_stopping_area_module - behavior_velocity_occlusion_spot_module - behavior_velocity_out_of_lane_module - behavior_velocity_run_out_module behavior_velocity_speed_bump_module - behavior_velocity_stop_line_module - behavior_velocity_traffic_light_module - behavior_velocity_walkway_module rosidl_interface_packages diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index ffbc4ef9174dc..40457ce4b30f3 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -14,8 +14,8 @@ #include "node.hpp" +#include #include -#include #include #include #include @@ -84,41 +84,13 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio createSubscriptionOptions(this)); // Subscribers - sub_predicted_objects_ = - this->create_subscription( - "~/input/dynamic_objects", 1, - std::bind(&BehaviorVelocityPlannerNode::onPredictedObjects, this, _1), - createSubscriptionOptions(this)); - sub_no_ground_pointcloud_ = this->create_subscription( - "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), - std::bind(&BehaviorVelocityPlannerNode::onNoGroundPointCloud, this, _1), - createSubscriptionOptions(this)); - sub_vehicle_odometry_ = this->create_subscription( - "~/input/vehicle_odometry", 1, std::bind(&BehaviorVelocityPlannerNode::onOdometry, this, _1), - createSubscriptionOptions(this)); - sub_acceleration_ = this->create_subscription( - "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1), - createSubscriptionOptions(this)); sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), createSubscriptionOptions(this)); - sub_traffic_signals_ = - this->create_subscription( - "~/input/traffic_signals", 1, - std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), - createSubscriptionOptions(this)); sub_external_velocity_limit_ = this->create_subscription( "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(), std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1)); - sub_virtual_traffic_light_states_ = - this->create_subscription( - "~/input/virtual_traffic_light_states", 1, - std::bind(&BehaviorVelocityPlannerNode::onVirtualTrafficLightStates, this, _1), - createSubscriptionOptions(this)); - sub_occupancy_grid_ = this->create_subscription( - "~/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)); @@ -178,66 +150,32 @@ void BehaviorVelocityPlannerNode::onUnloadPlugin( planner_manager_.removeScenePlugin(*this, request->plugin_name); } -// NOTE: argument planner_data must not be referenced for multithreading -bool BehaviorVelocityPlannerNode::isDataReady( - const PlannerData planner_data, rclcpp::Clock clock) const +void BehaviorVelocityPlannerNode::onParam() { - const auto & d = planner_data; - - // from callbacks - if (!d.current_odometry) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current odometry"); - return false; - } - - if (!d.current_velocity) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current velocity"); - return false; - } - if (!d.current_acceleration) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current acceleration"); - return false; - } - if (!d.predicted_objects) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for predicted_objects"); - return false; - } - if (!d.no_ground_pointcloud) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for pointcloud"); - return false; - } - if (!map_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for the initialization of map"); - return false; - } - if (!d.velocity_smoother_) { - RCLCPP_INFO_THROTTLE( - get_logger(), clock, 3000, "Waiting for the initialization of velocity smoother"); - return false; - } - if (!d.occupancy_grid) { - RCLCPP_INFO_THROTTLE( - get_logger(), clock, 3000, "Waiting for the initialization of occupancy grid map"); - return false; - } - return true; + // Note(VRichardJP): mutex lock is not necessary as onParam is only called once in the + // constructed. It would be required if it was a callback. std::lock_guard + // lock(mutex_); + planner_data_.velocity_smoother_ = + std::make_unique(*this); + planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); } -void BehaviorVelocityPlannerNode::onOccupancyGrid( - const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) +void BehaviorVelocityPlannerNode::onLaneletMap( + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { std::lock_guard lock(mutex_); - planner_data_.occupancy_grid = msg; + + map_ptr_ = msg; + has_received_map_ = true; } -void BehaviorVelocityPlannerNode::onPredictedObjects( - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) +void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) { std::lock_guard lock(mutex_); - planner_data_.predicted_objects = msg; + planner_data_.external_velocity_limit = *msg; } -void BehaviorVelocityPlannerNode::onNoGroundPointCloud( +void BehaviorVelocityPlannerNode::processNoGroundPointCloud( const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { geometry_msgs::msg::TransformStamped transform; @@ -258,16 +196,11 @@ void BehaviorVelocityPlannerNode::onNoGroundPointCloud( tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); } - { - std::lock_guard lock(mutex_); - planner_data_.no_ground_pointcloud = pc_transformed; - } + planner_data_.no_ground_pointcloud = pc_transformed; } -void BehaviorVelocityPlannerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void BehaviorVelocityPlannerNode::processOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { - std::lock_guard lock(mutex_); - auto current_odometry = std::make_shared(); current_odometry->header = msg->header; current_odometry->pose = msg->pose.pose; @@ -297,37 +230,9 @@ void BehaviorVelocityPlannerNode::onOdometry(const nav_msgs::msg::Odometry::Cons } } -void BehaviorVelocityPlannerNode::onAcceleration( - const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) -{ - std::lock_guard lock(mutex_); - planner_data_.current_acceleration = msg; -} - -void BehaviorVelocityPlannerNode::onParam() -{ - // Note(VRichardJP): mutex lock is not necessary as onParam is only called once in the - // constructed. It would be required if it was a callback. std::lock_guard - // lock(mutex_); - planner_data_.velocity_smoother_ = - std::make_unique(*this); - planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); -} - -void BehaviorVelocityPlannerNode::onLaneletMap( - const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) -{ - std::lock_guard lock(mutex_); - - map_ptr_ = msg; - has_received_map_ = true; -} - -void BehaviorVelocityPlannerNode::onTrafficSignals( +void BehaviorVelocityPlannerNode::processTrafficSignals( const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { - std::lock_guard lock(mutex_); - // clear previous observation planner_data_.traffic_light_id_map_raw_.clear(); const auto traffic_light_id_map_last_observed_old = @@ -361,17 +266,70 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( } } -void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) +bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) { - std::lock_guard lock(mutex_); - planner_data_.external_velocity_limit = *msg; + bool is_ready = true; + const auto & logData = [&clock, this](const std::string & data_type) { + std::string msg = "Waiting for " + data_type + " data"; + RCLCPP_INFO_THROTTLE(get_logger(), clock, logger_throttle_interval, msg.c_str()); + }; + + const auto & getData = [&logData](auto & dest, auto & sub, const std::string & data_type = "") { + const auto temp = sub.takeData(); + if (temp) { + dest = temp; + return true; + } + if (!data_type.empty()) logData(data_type); + return false; + }; + + is_ready &= getData(planner_data_.current_acceleration, sub_acceleration_, "acceleration"); + is_ready &= getData(planner_data_.predicted_objects, sub_predicted_objects_, "predicted_objects"); + is_ready &= getData(planner_data_.occupancy_grid, sub_occupancy_grid_, "occupancy_grid"); + + const auto odometry = sub_vehicle_odometry_.takeData(); + if (odometry) { + processOdometry(odometry); + } else { + logData("odometry"); + is_ready = false; + } + + const auto no_ground_pointcloud = sub_no_ground_pointcloud_.takeData(); + if (no_ground_pointcloud) { + processNoGroundPointCloud(no_ground_pointcloud); + } else { + logData("pointcloud"); + is_ready = false; + } + + // optional data + getData(planner_data_.virtual_traffic_light_states, sub_virtual_traffic_light_states_); + + const auto traffic_signals = sub_traffic_signals_.takeData(); + if (traffic_signals) processTrafficSignals(traffic_signals); + + return is_ready; } -void BehaviorVelocityPlannerNode::onVirtualTrafficLightStates( - const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg) +// NOTE: argument planner_data must not be referenced for multithreading +bool BehaviorVelocityPlannerNode::isDataReady(rclcpp::Clock clock) { - std::lock_guard lock(mutex_); - planner_data_.virtual_traffic_light_states = msg; + if (!planner_data_.velocity_smoother_) { + RCLCPP_INFO_THROTTLE( + get_logger(), clock, logger_throttle_interval, + "Waiting for the initialization of velocity smoother"); + return false; + } + + if (!map_ptr_) { + RCLCPP_INFO_THROTTLE( + get_logger(), clock, logger_throttle_interval, "Waiting for lanelet_map data"); + return false; + } + + return processData(clock); } void BehaviorVelocityPlannerNode::onTrigger( @@ -379,7 +337,7 @@ void BehaviorVelocityPlannerNode::onTrigger( { std::unique_lock lk(mutex_); - if (!isDataReady(planner_data_, *get_clock())) { + if (!isDataReady(*get_clock())) { return; } @@ -390,7 +348,8 @@ void BehaviorVelocityPlannerNode::onTrigger( } if (!planner_data_.route_handler_) { RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 3000, "Waiting for the initialization of route_handler"); + get_logger(), *get_clock(), logger_throttle_interval, + "Waiting for the initialization of route_handler"); return; } @@ -423,7 +382,7 @@ autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; if (!is_driving_forward_) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), 3000, + get_logger(), *get_clock(), logger_throttle_interval, "Backward path is NOT supported. just converting path_with_lane_id to path"); output_path_msg = to_path(*input_path_msg); output_path_msg.header.frame_id = "map"; @@ -439,14 +398,14 @@ autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( // screening const auto filtered_path = - ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); + autoware::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( + const auto interpolated_path_msg = autoware::behavior_velocity_planner::interpolatePath( filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point - output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); + output_path_msg = autoware::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp index 62ceef5f04ea6..73dfdb15e2d8d 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -17,10 +17,11 @@ #include "planner_manager.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include #include -#include +#include #include #include @@ -49,7 +50,6 @@ namespace autoware::behavior_velocity_planner using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; using autoware_map_msgs::msg::LaneletMapBin; -using ::behavior_velocity_planner::TrafficSignalStamped; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node @@ -65,33 +65,46 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr trigger_sub_path_with_lane_id_; - rclcpp::Subscription::SharedPtr - sub_predicted_objects_; - rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; - rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; - rclcpp::Subscription::SharedPtr sub_acceleration_; rclcpp::Subscription::SharedPtr sub_lanelet_map_; - rclcpp::Subscription::SharedPtr - sub_traffic_signals_; - rclcpp::Subscription::SharedPtr - sub_virtual_traffic_light_states_; - rclcpp::Subscription::SharedPtr sub_occupancy_grid_; rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; + // polling subscribers + tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_perception_msgs::msg::PredictedObjects> + sub_predicted_objects_{this, "~/input/dynamic_objects"}; + + tier4_autoware_utils::InterProcessPollingSubscriber + sub_no_ground_pointcloud_{this, "~/input/no_ground_pointcloud"}; + + tier4_autoware_utils::InterProcessPollingSubscriber + sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; + + tier4_autoware_utils::InterProcessPollingSubscriber< + geometry_msgs::msg::AccelWithCovarianceStamped> + sub_acceleration_{this, "~/input/accel"}; + + tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_perception_msgs::msg::TrafficLightGroupArray> + sub_traffic_signals_{this, "~/input/traffic_signals"}; + + tier4_autoware_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> + sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; + + tier4_autoware_utils::InterProcessPollingSubscriber + sub_occupancy_grid_{this, "~/input/occupancy_grid"}; + void onTrigger(const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); - void onPredictedObjects( - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); - void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + + void onParam(); void onLaneletMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); - void onTrafficSignals( - const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); - void onVirtualTrafficLightStates( - const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); - void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg); - void onParam(); + + void processNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void processOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void processTrafficSignals( + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); + bool processData(rclcpp::Clock clock); // publisher rclcpp::Publisher::SharedPtr path_pub_; @@ -124,8 +137,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::mutex mutex_; // function - geometry_msgs::msg::PoseStamped getCurrentPose(); - bool isDataReady(const PlannerData planner_data, rclcpp::Clock clock) const; + bool isDataReady(rclcpp::Clock clock); autoware_planning_msgs::msg::Path generatePath( const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); @@ -133,6 +145,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; + + static constexpr int logger_throttle_interval = 3000; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index f462fc963f17b..93209a10180be 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -50,7 +50,8 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } // namespace BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() -: plugin_loader_("autoware_behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") +: plugin_loader_( + "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") { } diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index 9e7f2942bb067..73193a002918d 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -15,8 +15,8 @@ #ifndef PLANNER_MANAGER_HPP_ #define PLANNER_MANAGER_HPP_ -#include -#include +#include +#include #include #include @@ -38,8 +38,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PlannerData; -using ::behavior_velocity_planner::PluginInterface; class BehaviorVelocityPlannerManager { diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 7cbe0f65a1fc2..e630783f82d6b 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -68,21 +68,20 @@ std::shared_ptr generateNode() }; std::vector module_names; - module_names.emplace_back("behavior_velocity_planner::CrosswalkModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::WalkwayModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::TrafficLightModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::IntersectionModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::MergeFromPrivateModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::BlindSpotModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::DetectionAreaModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::CrosswalkModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::WalkwayModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::TrafficLightModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::IntersectionModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::BlindSpotModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::DetectionAreaModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::NoStoppingAreaModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::StopLineModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::OcclusionSpotModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::RunOutModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::SpeedBumpModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::OutOfLaneModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::NoDrivableLaneModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::StopLineModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::OcclusionSpotModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::RunOutModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::SpeedBumpModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -96,19 +95,18 @@ std::shared_ptr generateNode() velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", velocity_smoother_dir + "/config/Analytical.param.yaml", behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config_no_prefix("blind_spot"), - get_behavior_velocity_module_config_no_prefix("crosswalk"), - get_behavior_velocity_module_config_no_prefix("walkway"), + get_behavior_velocity_module_config("blind_spot"), + get_behavior_velocity_module_config("crosswalk"), + get_behavior_velocity_module_config("walkway"), get_behavior_velocity_module_config_no_prefix("detection_area"), - get_behavior_velocity_module_config_no_prefix("intersection"), + get_behavior_velocity_module_config("intersection"), get_behavior_velocity_module_config_no_prefix("no_stopping_area"), - get_behavior_velocity_module_config_no_prefix("occlusion_spot"), - get_behavior_velocity_module_config_no_prefix("run_out"), + get_behavior_velocity_module_config("occlusion_spot"), + get_behavior_velocity_module_config("run_out"), get_behavior_velocity_module_config_no_prefix("speed_bump"), - get_behavior_velocity_module_config_no_prefix("stop_line"), - get_behavior_velocity_module_config_no_prefix("traffic_light"), + 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_no_prefix("out_of_lane"), get_behavior_velocity_module_config_no_prefix("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules diff --git a/planning/autoware_behavior_velocity_planner_common/CMakeLists.txt b/planning/autoware_behavior_velocity_planner_common/CMakeLists.txt new file mode 100644 index 0000000000000..9cb992312f52a --- /dev/null +++ b/planning/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_planner_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene_module_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 +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/src/test_state_machine.cpp + test/src/test_arc_lane_util.cpp + test/src/test_utilization.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + gtest_main + ${PROJECT_NAME} + ) +endif() + +ament_auto_package() diff --git a/planning/behavior_velocity_planner_common/README.md b/planning/autoware_behavior_velocity_planner_common/README.md similarity index 100% rename from planning/behavior_velocity_planner_common/README.md rename to planning/autoware_behavior_velocity_planner_common/README.md diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp similarity index 86% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp index 51511b94f3e33..068a10b7e16f3 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ -#include "route_handler/route_handler.hpp" +#include "autoware_route_handler/route_handler.hpp" +#include +#include #include -#include -#include #include #include @@ -45,13 +45,13 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class BehaviorVelocityPlannerNode; struct PlannerData { explicit PlannerData(rclcpp::Node & node) - : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) + : vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { max_stop_acceleration_threshold = node.declare_parameter( "max_accel"); // TODO(someone): read min_acc in velocity_controller.param.yaml? @@ -88,11 +88,11 @@ struct PlannerData bool is_simulation = false; // velocity smoother - std::shared_ptr velocity_smoother_; + std::shared_ptr velocity_smoother_; // route handler - std::shared_ptr route_handler_; + std::shared_ptr route_handler_; // parameters - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; // additional parameters double max_stop_acceleration_threshold; @@ -148,6 +148,6 @@ struct PlannerData return std::make_optional(traffic_light_id_map.at(id)); } }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp similarity index 75% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp index dcdb4a7052cc0..86579f06790b2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ -#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class PluginInterface @@ -38,6 +38,6 @@ class PluginInterface virtual const char * getModuleName() = 0; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp similarity index 78% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp index abb14dd8b2356..e82211937e55b 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { template @@ -48,6 +48,6 @@ class PluginWrapper : public PluginInterface std::unique_ptr scene_manager_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp new file mode 100644 index 0000000000000..bc929991199ac --- /dev/null +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp @@ -0,0 +1,281 @@ +// 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 AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +// Debug +#include + +#include +namespace autoware::behavior_velocity_planner +{ + +using autoware::objects_of_interest_marker_interface::ColorName; +using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using autoware::rtc_interface::RTCInterface; +using builtin_interfaces::msg::Time; +using motion_utils::PlanningBehavior; +using motion_utils::VelocityFactor; +using tier4_autoware_utils::DebugPublisher; +using tier4_autoware_utils::getOrDeclareParameter; +using tier4_debug_msgs::msg::Float64Stamped; +using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; +using tier4_rtc_msgs::msg::Module; +using tier4_rtc_msgs::msg::State; +using unique_identifier_msgs::msg::UUID; + +struct ObjectOfInterest +{ + geometry_msgs::msg::Pose pose; + autoware_perception_msgs::msg::Shape shape; + ColorName color; + ObjectOfInterest( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, + const ColorName & color_name) + : pose(pose), shape(shape), color(color_name) + { + } +}; + +class SceneModuleInterface +{ +public: + explicit SceneModuleInterface( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + virtual ~SceneModuleInterface() = default; + + virtual bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) = 0; + + virtual visualization_msgs::msg::MarkerArray createDebugMarkerArray() = 0; + virtual std::vector createVirtualWalls() = 0; + + int64_t getModuleId() const { return module_id_; } + void setPlannerData(const std::shared_ptr & planner_data) + { + planner_data_ = planner_data; + } + + std::optional getInfrastructureCommand() + { + return infrastructure_command_; + } + + void setInfrastructureCommand( + const std::optional & command) + { + infrastructure_command_ = command; + } + + std::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } + + void setActivation(const bool activated) { activated_ = activated; } + void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } + bool isActivated() const { return activated_; } + bool isSafe() const { return safe_; } + double getDistance() const { return distance_; } + + void resetVelocityFactor() { velocity_factor_.reset(); } + VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } + std::vector getObjectsOfInterestData() const { return objects_of_interest_; } + void clearObjectsOfInterestData() { objects_of_interest_.clear(); } + +protected: + const int64_t module_id_; + bool activated_; + bool safe_; + bool rtc_enabled_; + double distance_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr planner_data_; + std::optional infrastructure_command_; + std::optional first_stop_path_point_index_; + motion_utils::VelocityFactorInterface velocity_factor_; + std::vector objects_of_interest_; + + void setSafe(const bool safe) + { + safe_ = safe; + if (!rtc_enabled_) { + syncActivation(); + } + } + void setDistance(const double distance) { distance_ = distance; } + void syncActivation() { setActivation(isSafe()); } + + void setObjectsOfInterestData( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, + const ColorName & color_name) + { + objects_of_interest_.emplace_back(pose, shape, color_name); + } + + size_t findEgoSegmentIndex( + const std::vector & points) const; +}; + +class SceneModuleManagerInterface +{ +public: + SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name); + + virtual ~SceneModuleManagerInterface() = default; + + virtual const char * getModuleName() = 0; + + std::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } + + void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); + + virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } + +protected: + virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path); + + virtual void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + + virtual std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; + + virtual void deleteExpiredModules(const tier4_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); + + void unregisterModule(const std::shared_ptr & scene_module); + + size_t findEgoSegmentIndex( + const std::vector & points) const; + + std::set> scene_modules_; + std::set registered_module_id_set_; + + std::shared_ptr planner_data_; + motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_; + + std::optional first_stop_path_point_index_; + rclcpp::Node & node_; + rclcpp::Clock::SharedPtr clock_; + // Debug + bool is_publish_debug_path_ = {false}; // note : this is very heavy debug topic option + rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; + rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr pub_debug_path_; + rclcpp::Publisher::SharedPtr pub_stop_reason_; + rclcpp::Publisher::SharedPtr + pub_velocity_factor_; + rclcpp::Publisher::SharedPtr + pub_infrastructure_commands_; + + std::shared_ptr processing_time_publisher_; +}; + +class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface +{ +public: + SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); + + void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; + +protected: + RTCInterface rtc_interface_; + std::unordered_map map_uuid_; + + ObjectsOfInterestMarkerInterface objects_of_interest_marker_interface_; + + virtual void sendRTC(const Time & stamp); + + virtual void setActivation(); + + void updateRTCStatus( + const UUID & uuid, const bool safe, const uint8_t state, const double distance, + const Time & stamp) + { + rtc_interface_.updateCooperateStatus(uuid, safe, state, distance, distance, stamp); + } + + void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); } + + void publishRTCStatus(const Time & stamp) + { + rtc_interface_.removeExpiredCooperateStatus(); + rtc_interface_.publishCooperateStatus(stamp); + } + + UUID getUUID(const int64_t & module_id) const; + + void generateUUID(const int64_t & module_id); + + void removeUUID(const int64_t & module_id); + + void publishObjectsOfInterestMarker(); + + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) + { + bool enable_rtc = true; + + try { + enable_rtc = getOrDeclareParameter(node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(node, param_name); + } catch (const std::exception & e) { + enable_rtc = getOrDeclareParameter(node, param_name); + } + + return enable_rtc; + } +}; + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp similarity index 94% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp index c9d292536ac13..5ea1ae9fffcc1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ -#include +#include #include #include @@ -27,7 +27,7 @@ #define EIGEN_MPL2_ONLY #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -195,6 +195,6 @@ std::optional createTargetPoint( const size_t lane_id, const double margin, const double vehicle_offset); } // namespace arc_lane_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp similarity index 88% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index bf238ecad55cb..22bba2b90dcc2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ #include @@ -46,7 +46,7 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, pose.position.y, pose.position.z) -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -77,6 +77,6 @@ Polygon2d upScalePolygon( geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp new file mode 100644 index 0000000000000..c6e5d45ec4eb6 --- /dev/null +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp @@ -0,0 +1,51 @@ +// 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 AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::behavior_velocity_planner +{ +namespace debug +{ +visualization_msgs::msg::MarkerArray createPolygonMarkerArray( + const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t module_id, + const rclcpp::Time & now, const double x, const double y, const double z, const double r, + const double g, const double b); +visualization_msgs::msg::MarkerArray createPathMarkerArray( + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b); +visualization_msgs::msg::MarkerArray createObjectsMarkerArray( + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double r, const double g, + const double b); +visualization_msgs::msg::MarkerArray createPointsMarkerArray( + const std::vector & points, const std::string & ns, + const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, + const double r, const double g, const double b); +} // namespace debug +} // namespace autoware::behavior_velocity_planner +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp similarity index 78% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp index 55a82db1ae390..e9bccc42e1dc4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ #include @@ -22,7 +22,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, @@ -33,6 +33,6 @@ autoware_planning_msgs::msg::Path filterLitterPathPoint( const autoware_planning_msgs::msg::Path & path); autoware_planning_msgs::msg::Path filterStopPathPoint( const autoware_planning_msgs::msg::Path & path); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp new file mode 100644 index 0000000000000..12dd4db930745 --- /dev/null +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp @@ -0,0 +1,96 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +/** + * @brief Manage stop-go states with safety margin time. + */ +class StateMachine +{ +public: + enum State { + STOP = 0, + GO, + }; + static std::string toString(const State & state) + { + if (state == State::STOP) { + return "STOP"; + } else if (state == State::GO) { + return "GO"; + } else { + return ""; + } + } + StateMachine() + { + state_ = State::GO; + margin_time_ = 0.0; + duration_ = 0.0; + } + void setStateWithMarginTime(State state, rclcpp::Logger logger, rclcpp::Clock & clock) + { + /* same state request */ + if (state_ == state) { + start_time_ = nullptr; // reset timer + return; + } + + /* GO -> STOP */ + if (state == State::STOP) { + state_ = State::STOP; + start_time_ = nullptr; // reset timer + return; + } + + /* STOP -> GO */ + if (state == State::GO) { + if (start_time_ == nullptr) { + start_time_ = std::make_shared(clock.now()); + } else { + duration_ = (clock.now() - *start_time_).seconds(); + if (duration_ > margin_time_) { + state_ = State::GO; + start_time_ = nullptr; // reset timer + } + } + return; + } + RCLCPP_ERROR(logger, "Unsuitable state. ignore request."); + } + + void setMarginTime(const double t) { margin_time_ = t; } + void setState(State state) { state_ = state; } + State getState() const { return state_; } + double getDuration() const { return duration_; } + +private: + State state_; //! current state + double margin_time_; //! margin time when transit to Go from Stop + double duration_; //! duration time when transit to Go from Stop + std::shared_ptr start_time_; //! first time received GO when STOP state +}; + +} // namespace autoware::behavior_velocity_planner +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp new file mode 100644 index 0000000000000..c5040f055c243 --- /dev/null +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -0,0 +1,45 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ + +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathWithLaneId; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Quaternion; +using TrajectoryPointWithIdx = std::pair; + +//! 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); + +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp new file mode 100644 index 0000000000000..2fe728847b8f2 --- /dev/null +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp @@ -0,0 +1,246 @@ +// Copyright 2015-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. + +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +struct DetectionRange +{ + bool use_right = true; + bool use_left = true; + double interval; + double min_longitudinal_distance; + double max_longitudinal_distance; + double max_lateral_distance; + double wheel_tread; + double right_overhang; + double left_overhang; +}; + +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; + autoware_perception_msgs::msg::TrafficLightGroup signal; +}; + +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_perception_msgs::msg::PredictedObjects; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; + +namespace planning_utils +{ +size_t calcSegmentIndexFromPointIndex( + 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); +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); +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys); +void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input); +void insertVelocity( + PathWithLaneId & path, const PathPointWithLaneId & path_point, const double v, + size_t & insert_index, const double min_distance = 0.001); +inline int64_t bitShift(int64_t original_id) +{ + return original_id << (sizeof(int32_t) * 8 / 2); +} + +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, + const tier4_planning_msgs::msg::PathWithLaneId & path); +Polygon2d generatePathPolygon( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); +double calcJudgeLineDistWithAccLimit( + const double velocity, const double max_stop_acceleration, const double delay_response_time); + +double calcJudgeLineDistWithJerkLimit( + const double velocity, const double acceleration, const double max_stop_acceleration, + const double max_stop_jerk, const double delay_response_time); + +double calcDecelerationVelocityFromDistanceToTarget( + const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel, + const double current_velocity, const double distance_to_target); + +double findReachTime( + const double jerk, const double accel, const double velocity, const double distance, + const double t_min, const double t_max); + +StopReason initializeStopReason(const std::string & stop_reason); + +void appendStopReason(const StopFactor stop_factor, StopReason * stop_reason); + +std::vector toRosPoints(const PredictedObjects & object); + +LineString2d extendLine( + const lanelet::ConstPoint3d & lanelet_point1, const lanelet::ConstPoint3d & lanelet_point2, + const double & length); + +template +std::vector concatVector(const std::vector & vec1, const std::vector & vec2) +{ + auto concat_vec = vec1; + concat_vec.insert(std::end(concat_vec), std::begin(vec2), std::end(vec2)); + return concat_vec; +} + +std::optional getNearestLaneId( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); + +std::vector getSortedLaneIdsFromPath(const PathWithLaneId & path); + +// return the set of lane_ids in the path after base_lane_id +std::vector getSubsequentLaneIdsSetOnPath( + const PathWithLaneId & path, int64_t base_lane_id); + +template +std::unordered_map, lanelet::ConstLanelet> getRegElemMapOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::unordered_map, lanelet::ConstLanelet> reg_elem_map_on_path; + + // Add current lane id + const auto nearest_lane_id = getNearestLaneId(path, lanelet_map, current_pose); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + // Add subsequent lane_ids from nearest lane_id + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); + } else { + // Add all lane_ids in path + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + } + + for (const auto lane_id : unique_lane_ids) { + const auto ll = lanelet_map->laneletLayer.get(lane_id); + + for (const auto & reg_elem : ll.regulatoryElementsAs()) { + reg_elem_map_on_path.insert(std::make_pair(reg_elem, ll)); + } + } + + return reg_elem_map_on_path; +} + +template +std::set getRegElemIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::set reg_elem_id_set; + for (const auto & m : getRegElemMapOnPath(path, lanelet_map, current_pose)) { + reg_elem_id_set.insert(m.first->id()); + } + return reg_elem_id_set; +} + +template +std::set getLaneletIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose) +{ + std::set id_set; + for (const auto & m : getRegElemMapOnPath(path, lanelet_map, current_pose)) { + id_set.insert(m.second.id()); + } + return id_set; +} + +std::optional insertDecelPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output, + const float target_velocity); + +std::vector getLaneletsOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); + +std::set getLaneIdSetOnPath( + const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, + const geometry_msgs::msg::Pose & current_pose); + +bool isOverLine( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const double offset = 0.0); + +std::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); +std::optional insertStopPoint( + const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output); + +/* + @brief return 'associative' lanes in the intersection. 'associative' means that a lane shares same + or lane-changeable parent lanes with `lane` and has same turn_direction value. + */ +std::set getAssociativeIntersectionLanelets( + lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map, + const lanelet::routing::RoutingGraphPtr routing_graph); + +template